(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 10.2' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[    589410,      11950]
NotebookOptionsPosition[    586783,      11873]
NotebookOutlinePosition[    587121,      11888]
CellTagsIndexPosition[    587078,      11885]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[TextData[{
 StyleBox["Library of functions written to accompany the algorithms described \
in ",
  FontSize->36,
  FontColor->RGBColor[0, 0, 1]],
 StyleBox["Introduction to Robotics: Mechanics, Planning, and Control",
  FontSize->36,
  FontSlant->"Italic",
  FontColor->RGBColor[0, 0, 1]],
 StyleBox[".",
  FontSize->36,
  FontColor->RGBColor[0, 0, 1]]
}], "Title",
 CellChangeTimes->{{3.658271341588306*^9, 3.6582713456457853`*^9}, {
   3.675528750549493*^9, 3.675528779907896*^9}, {3.675529226872184*^9, 
   3.67552922870382*^9}, {3.675529362176085*^9, 3.675529365448338*^9}, 
   3.675529396890761*^9, {3.676205646014924*^9, 3.6762056476708612`*^9}}],

Cell[TextData[{
 StyleBox["Author: Mikhail Todes\nEmail: mikhail@u.northwestern.edu\nGithub: \
https://github.com/MikhailTodes\nFunctions also available in: Python, MATLAB\n\
\n",
  FontSize->18,
  FontColor->RGBColor[0, 0, 1]],
 StyleBox["Notes:\n",
  FontSize->18,
  FontWeight->"Bold",
  FontColor->RGBColor[0, 0, 1]],
 StyleBox["Before using any of the functions in this library, each chapter\
\[CloseCurlyQuote]s cell needs to be evaluated. This can be done as a whole \
by highlighting everything (Ctrl + A) and then evaluating (Shift + Enter). \n\
\nDouble click on the tabs at the side of each chapter to expand and view the \
code.\n\nAt the start of each function is a description of what it does as \
well as an example showing how to use it.\nQuick tip: If you see a small T \
after braces, it means the transpose of that vector. This can be done the \
same way as accessing Greek letters in Mathematica. Press the Esc key, type \
\[OpenCurlyDoubleQuote]tr\[CloseCurlyDoubleQuote], and then press the Esc key \
again.",
  FontSize->18,
  FontColor->RGBColor[0, 0, 1]],
 StyleBox[" ",
  FontSize->18,
  FontWeight->"Bold",
  FontColor->RGBColor[0, 0, 1]]
}], "Title",
 CellChangeTimes->{{3.658271341588306*^9, 3.6582713456457853`*^9}, {
   3.675528750549493*^9, 3.675528779907896*^9}, {3.675529226872184*^9, 
   3.67552922870382*^9}, {3.675529362176085*^9, 3.675529365448338*^9}, 
   3.675529396890761*^9, {3.676205646014924*^9, 3.676205711454337*^9}, {
   3.676205798666971*^9, 3.676205801603088*^9}, {3.6786303129136467`*^9, 
   3.678630314739846*^9}, 3.682543797297825*^9, {3.6832314306368*^9, 
   3.683231490482093*^9}, {3.683231539283634*^9, 3.683231632134388*^9}, {
   3.683232465039819*^9, 3.6832324695390797`*^9}, {3.683232533035797*^9, 
   3.683232573144146*^9}}],

Cell[BoxData["\[IndentingNewLine]"], "Input",
 CellChangeTimes->{
  3.675532098370377*^9, {3.676113418387143*^9, 3.676113423296434*^9}, {
   3.676114752897053*^9, 3.676114755535831*^9}, 3.683231333358562*^9, {
   3.683231417735001*^9, 3.683231421228323*^9}, 3.683232620167864*^9, {
   3.6833083588497458`*^9, 3.68330836588066*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  3: RIGID-BODY MOTION",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.6756152664138308`*^9, 
  3.675615299230091*^9}, {3.6761156821135902`*^9, 3.676115684953891*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"RotInv", "[", "R_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "a", " ", "3", "x3", " ", "rotation", " ", "matrix", " ", 
      "and", " ", "returns", " ", "the", " ", "transpose", " ", 
      RowBox[{
       RowBox[{"(", "inverse", ")"}], "."}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "invR"}], " ", 
     "=", " ", 
     RowBox[{
      RowBox[{"RotInv", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "}"}], "]"}], 
      "\[IndentingNewLine]", 
      RowBox[{"Output", ":", " ", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "}"}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"RotInv", "[", "R_", "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", "}"}], ",", 
      RowBox[{"If", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{"R", "\[Transpose]"}], ".", "R"}], "\[Equal]", 
          RowBox[{"IdentityMatrix", "[", "3", "]"}]}], " ", "&&", " ", 
         RowBox[{
          RowBox[{"Det", "[", "R", "]"}], "\[Equal]", "1"}]}], ",", 
        RowBox[{"R", "\[Transpose]"}], ",", 
        RowBox[{"Print", "[", "\"\<Inverse cannot be calculated\>\"", "]"}]}],
        "]"}]}], "]"}]}], "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
   3.675531958735077*^9, 3.675532032779791*^9}, {3.6755321540719852`*^9, 
   3.675532155394449*^9}, 3.67559226134205*^9, {3.675596340559066*^9, 
   3.675596342576479*^9}, {3.678095916353695*^9, 3.678095936057015*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"VecToso3", "[", "omg_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "a", " ", "3"}], "-", 
      RowBox[{"vector", 
       RowBox[{"(", 
        RowBox[{"angular", " ", "velocity"}], ")"}], " ", "and", " ", 
       "returns", " ", "the", " ", "skew", " ", "symmetric", " ", "matrix", 
       " ", "in", " ", 
       RowBox[{"so3", "."}]}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "so3mat"}], 
     " ", "=", " ", 
     RowBox[{
      RowBox[{"VecToso3", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"{", 
          RowBox[{"1", ",", "2", ",", "3"}], "}"}], "}"}], "\[Transpose]"}], 
       "]"}], "\[IndentingNewLine]", 
      RowBox[{"Output", ":", " ", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "3"}], ",", "2"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"3", ",", "0", ",", 
           RowBox[{"-", "1"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", "2"}], ",", "1", ",", "0"}], "}"}]}], "}"}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"VecToso3", "[", "omg_", "]"}], ":=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", 
        RowBox[{"-", 
         RowBox[{"omg", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "1"}], "]"}], "]"}]}], ",", 
        RowBox[{"omg", "[", 
         RowBox[{"[", 
          RowBox[{"2", ",", "1"}], "]"}], "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"omg", "[", 
         RowBox[{"[", 
          RowBox[{"3", ",", "1"}], "]"}], "]"}], ",", "0", ",", 
        RowBox[{"-", 
         RowBox[{"omg", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "1"}], "]"}], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"omg", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "1"}], "]"}], "]"}]}], ",", 
        RowBox[{"omg", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "1"}], "]"}], "]"}], ",", "0"}], "}"}]}], "}"}]}],
    "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
   3.675531958735077*^9, 3.675532032779791*^9}, {3.6755321540719852`*^9, 
   3.675532176089467*^9}, {3.6755325631830273`*^9, 3.6755326462949533`*^9}, {
   3.6755327460483017`*^9, 3.675532746848569*^9}, {3.675533015887747*^9, 
   3.6755331043631153`*^9}, {3.675533262027841*^9, 3.6755332644293537`*^9}, 
   3.675592257047958*^9, 3.675594217872285*^9, {3.67559635411017*^9, 
   3.6755963639568*^9}, {3.678095931635499*^9, 3.678095941539589*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"so3ToVec", "[", "so3mat_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "a", " ", "3", "x3", " ", "skew"}], "-", 
      RowBox[{"symmetric", " ", "matrix", " ", 
       RowBox[{"(", 
        RowBox[{"an", " ", "element", " ", "of", " ", "so", 
         RowBox[{"(", "3", ")"}]}], ")"}], " ", "and", " ", "returns", " ", 
       "the", " ", "corresponding", " ", "vector", " ", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"angular", " ", "velocity"}], ")"}], "."}]}]}]}], 
    "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "omg"}], " ", 
     "=", " ", 
     RowBox[{
      RowBox[{"so3ToVec", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "3"}], ",", "2"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"3", ",", "0", ",", 
           RowBox[{"-", "1"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", "2"}], ",", "1", ",", "0"}], "}"}]}], "}"}], "]"}], 
      "\[IndentingNewLine]", 
      RowBox[{"Output", ":", " ", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", "1", "}"}], ",", 
         RowBox[{"{", "2", "}"}], ",", 
         RowBox[{"{", "3", "}"}]}], "}"}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"so3ToVec", "[", "so3mat_", "]"}], ":=", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"so3mat", "[", 
         RowBox[{"[", 
          RowBox[{"3", ",", "2"}], "]"}], "]"}], ",", 
        RowBox[{"so3mat", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
        RowBox[{"so3mat", "[", 
         RowBox[{"[", 
          RowBox[{"2", ",", "1"}], "]"}], "]"}]}], "}"}], "}"}], 
     "\[Transpose]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.675533307004715*^9, 3.67553330945144*^9}, 
   3.675592067518145*^9, {3.675592113013286*^9, 3.675592115100141*^9}, {
   3.675592155090198*^9, 3.675592250827938*^9}, {3.67559229043678*^9, 
   3.6755922914617233`*^9}, 3.675594212914967*^9, {3.67559639441064*^9, 
   3.675596396661991*^9}, {3.67809595187551*^9, 3.678095953534129*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"AxisAng3", "[", "expc3_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "A", " ", "3"}], "-", 
      RowBox[{
      "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", "for",
        " ", "rotation", " ", "and", " ", "returns", " ", "unit", " ", 
       "rotation", " ", "axis", " ", "omghat", " ", "and", " ", "the", " ", 
       "corresponding", " ", "rotation", " ", "angle", " ", 
       RowBox[{"theta", ".", "\[IndentingNewLine]", "The"}], " ", "first", 
       " ", "element", " ", "of", " ", "the", " ", "output", " ", "is", " ", 
       "the", " ", "unit", " ", "rotation", " ", "axis", " ", "column", " ", 
       "vector", " ", 
       RowBox[{"(", "omghat", ")"}], " ", "and", " ", "the", " ", "second", 
       " ", "element", " ", "is", " ", 
       RowBox[{"theta", "."}]}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", 
      RowBox[{"{", 
       RowBox[{"omghat", ",", " ", "theta"}], "}"}]}], " ", "=", " ", 
     RowBox[{
      RowBox[{"AxisAng3", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"{", 
          RowBox[{"1", ",", "2", ",", "3"}], "}"}], "}"}], "\[Transpose]"}], 
       "]"}], "//", 
      RowBox[{"N", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{
            RowBox[{"{", "0.2672612419124244`", "}"}], ",", 
            RowBox[{"{", "0.5345224838248488`", "}"}], ",", 
            RowBox[{"{", "0.8017837257372732`", "}"}]}], "}"}], ",", 
          "3.7416573867739413`"}], "}"}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"AxisAng3", "[", "expc3_", "]"}], ":=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"expc3", "/", 
       RowBox[{"Norm", "[", "expc3", "]"}]}], ",", 
      RowBox[{"Norm", "[", "expc3", "]"}]}], "}"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.675533307004715*^9, 3.67553330945144*^9}, 
   3.675592067518145*^9, {3.675592113013286*^9, 3.675592115100141*^9}, {
   3.675592155090198*^9, 3.675592250827938*^9}, {3.67559229043678*^9, 
   3.6755922914617233`*^9}, 3.6755923257071743`*^9, {3.675592392796273*^9, 
   3.6755924786666107`*^9}, {3.675592636879244*^9, 3.675592662642579*^9}, {
   3.675592692874652*^9, 3.675592696674159*^9}, 3.675594207807824*^9, 
   3.675596301390246*^9, 3.678095963162088*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{"MatrixExp3", "[", "expc3_", "]"}], ":", "\[IndentingNewLine]", 
      
      RowBox[{
       RowBox[{"Takes", " ", "a", " ", "3"}], "-", 
       RowBox[{
       "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", 
        "and", " ", "returns", " ", "R", " ", 
        RowBox[{"(", 
         RowBox[{"SO", 
          RowBox[{"(", "3", ")"}]}], ")"}], " ", "that", " ", "is", " ", 
        "achieved", " ", "by", " ", "rotating", " ", "about", " ", "omghat", 
        " ", "by", " ", "theta", " ", "from", " ", "an", " ", "initial", " ", 
        "orientation", " ", "R"}]}]}], "=", 
     RowBox[{
      RowBox[{"I", "\n", "Rodriguez", " ", "R"}], "=", 
      RowBox[{"I", "+", 
       RowBox[{"sin", 
        RowBox[{"(", "theta", ")"}], "*", "omghat"}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"1", "-", 
          RowBox[{"cos", 
           RowBox[{"(", "theta", ")"}]}]}], ")"}], "*", 
        RowBox[{"omghat", "^", "2"}]}]}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "R"}], 
     " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"MatrixExp3", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"1", ",", "2", ",", "3"}], "}"}], "}"}], "\[Transpose]"}], 
        "]"}], "//", "N"}], "//", 
      RowBox[{"MatrixForm", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {
             RowBox[{"-", "0.6949205576413118"}], "0.7135209905277876", 
             "0.08929285886191213"},
            {
             RowBox[{"-", "0.1920069727919994"}], 
             RowBox[{"-", "0.3037850443394705"}], "0.9331923538236468"},
            {"0.6929781677417701", "0.6313496993837178", "0.34810747783026474"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"MatrixExp3", "[", "expc3_", "]"}], ":=", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], "+", 
     RowBox[{
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{"AxisAng3", "[", "expc3", "]"}], "[", 
        RowBox[{"[", "2", "]"}], "]"}], "]"}], "*", 
      RowBox[{"VecToso3", "[", 
       RowBox[{
        RowBox[{"AxisAng3", "[", "expc3", "]"}], "[", 
        RowBox[{"[", "1", "]"}], "]"}], "]"}]}], "+", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"1", "-", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"AxisAng3", "[", "expc3", "]"}], "[", 
          RowBox[{"[", "2", "]"}], "]"}], "]"}]}], ")"}], "*", 
      RowBox[{"MatrixPower", "[", 
       RowBox[{
        RowBox[{"VecToso3", "[", 
         RowBox[{
          RowBox[{"AxisAng3", "[", "expc3", "]"}], "[", 
          RowBox[{"[", "1", "]"}], "]"}], "]"}], ",", "2"}], "]"}]}]}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.675533307004715*^9, 3.67553330945144*^9}, 
   3.675592067518145*^9, {3.675592113013286*^9, 3.675592115100141*^9}, {
   3.675592155090198*^9, 3.675592250827938*^9}, {3.67559229043678*^9, 
   3.6755922914617233`*^9}, 3.6755923257071743`*^9, {3.675592392796273*^9, 
   3.6755924786666107`*^9}, {3.675592636879244*^9, 3.675592662642579*^9}, {
   3.675592750871451*^9, 3.675592760234817*^9}, {3.675592843016467*^9, 
   3.6755928498639193`*^9}, {3.675593005317328*^9, 3.675593006955986*^9}, {
   3.675593085055738*^9, 3.675593106872654*^9}, {3.6755931552286654`*^9, 
   3.675593202588101*^9}, {3.675593410152174*^9, 3.675593425469223*^9}, {
   3.675593463962451*^9, 3.675593465912236*^9}, 3.675594202040379*^9, {
   3.675596420655054*^9, 3.675596421318139*^9}, {3.675601785945984*^9, 
   3.67560180540821*^9}, 3.678095974651895*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"MatrixLog3", "[", "R_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "R", " ", 
       RowBox[{"(", 
        RowBox[{"rotation", " ", "matrix"}], ")"}], " ", "and", " ", 
       "returns", " ", "the", " ", "corresponding", " ", "3"}], "-", 
      RowBox[{
      "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"expc3", "=", 
          RowBox[{"omghat", "*", "theta"}]}], ")"}], "."}]}]}]}], "  ", 
    "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "expc3"}], " ",
      "=", " ", 
     RowBox[{
      RowBox[{"MatrixLog3", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "}"}], "]"}], "//", 
      RowBox[{"N", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "1.2091995761561452", "}"}], ",", 
          RowBox[{"{", "1.2091995761561452", "}"}], ",", 
          RowBox[{"{", "1.2091995761561452", "}"}]}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"MatrixLog3", "[", "R_", "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"expc3", ",", "theta"}], "}"}], ",", 
      RowBox[{
       RowBox[{"theta", "=", 
        RowBox[{"ArcCos", "[", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{"Tr", "[", "R", "]"}], "-", "1"}], ")"}], "/", "2"}], 
         "]"}]}], ";", 
       RowBox[{"Which", "[", 
        RowBox[{
         RowBox[{"theta", "\[Equal]", "0"}], ",", 
         RowBox[{"expc3", "=", 
          RowBox[{
          "{", "\"\<Undefined - Identity therefore there is no rotation\>\"", 
           "}"}]}], ",", 
         RowBox[{
          RowBox[{"Tr", "[", "R", "]"}], "\[Equal]", 
          RowBox[{"-", "1"}]}], ",", 
         RowBox[{"Which", "[", 
          RowBox[{
           RowBox[{
            RowBox[{"1", "+", 
             RowBox[{"R", "[", 
              RowBox[{"[", 
               RowBox[{"3", ",", "3"}], "]"}], "]"}]}], "\[NotEqual]", " ", 
            "0"}], ",", 
           RowBox[{"expc3", "=", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"Sqrt", "[", 
               RowBox[{"2", 
                RowBox[{"(", 
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
              "*", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"2", ",", "3"}], "]"}], "]"}], ",", 
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}]}], "}"}], "}"}], 
              "\[Transpose]"}], "*", "Pi"}]}], ",", 
           RowBox[{
            RowBox[{"1", "+", 
             RowBox[{"R", "[", 
              RowBox[{"[", 
               RowBox[{"2", ",", "2"}], "]"}], "]"}]}], "\[NotEqual]", "0"}], 
           ",", 
           RowBox[{"expc3", "=", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"Sqrt", "[", 
               RowBox[{"2", 
                RowBox[{"(", 
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
              "*", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"1", ",", "2"}], "]"}], "]"}], ",", 
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ",", 
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"3", ",", "2"}], "]"}], "]"}]}], "}"}], "}"}], 
              "\[Transpose]"}], "*", "Pi"}]}], ",", 
           RowBox[{
            RowBox[{"1", "+", 
             RowBox[{"R", "[", 
              RowBox[{"[", 
               RowBox[{"1", ",", "1"}], "]"}], "]"}]}], "\[NotEqual]", " ", 
            "0"}], ",", 
           RowBox[{"expc3", "=", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"Sqrt", "[", 
               RowBox[{"2", 
                RowBox[{"(", 
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
              "*", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{"1", "+", 
                  RowBox[{"R", "[", 
                   RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ",", 
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
                 RowBox[{"R", "[", 
                  RowBox[{"[", 
                   RowBox[{"3", ",", "1"}], "]"}], "]"}]}], "}"}], "}"}], 
              "\[Transpose]"}], "*", "Pi"}]}]}], "]"}], ",", 
         RowBox[{
          RowBox[{"theta", "\[NotEqual]", "0"}], "&&", 
          RowBox[{
           RowBox[{"Tr", "[", "R", "]"}], "\[NotEqual]", 
           RowBox[{"-", "1"}]}]}], ",", 
         RowBox[{"expc3", "=", 
          RowBox[{
           RowBox[{"so3ToVec", "[", 
            RowBox[{
             RowBox[{"(", 
              RowBox[{"R", "-", 
               RowBox[{"R", "\[Transpose]"}]}], ")"}], "/", 
             RowBox[{"(", 
              RowBox[{"2", "*", 
               RowBox[{"Sin", "[", "theta", "]"}]}], ")"}]}], "]"}], "*", 
           "theta"}]}]}], "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "expc3", "]"}]}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.675533307004715*^9, 3.67553330945144*^9}, 
   3.675592067518145*^9, {3.675592113013286*^9, 3.675592115100141*^9}, {
   3.675592155090198*^9, 3.675592250827938*^9}, {3.67559229043678*^9, 
   3.6755922914617233`*^9}, 3.6755923257071743`*^9, {3.675592392796273*^9, 
   3.6755924786666107`*^9}, {3.675592636879244*^9, 3.675592662642579*^9}, {
   3.675592750871451*^9, 3.675592760234817*^9}, {3.675592843016467*^9, 
   3.6755928498639193`*^9}, {3.675593005317328*^9, 3.675593006955986*^9}, {
   3.675593085055738*^9, 3.675593106872654*^9}, {3.6755931552286654`*^9, 
   3.675593202588101*^9}, {3.675593410152174*^9, 3.675593425469223*^9}, {
   3.675593463962451*^9, 3.675593465912236*^9}, {3.675594236154458*^9, 
   3.6755942477744513`*^9}, {3.675594364598016*^9, 3.675594367233914*^9}, {
   3.675594505282823*^9, 3.675594524684219*^9}, {3.675594705253235*^9, 
   3.67559470565687*^9}, {3.675594770197138*^9, 3.6755948067599487`*^9}, {
   3.67559486980615*^9, 3.6755949272469254`*^9}, {3.675594959952829*^9, 
   3.675594966081294*^9}, {3.675595073310274*^9, 3.675595099122567*^9}, {
   3.675595166588663*^9, 3.675595198087933*^9}, {3.675596440163121*^9, 
   3.6755964490162983`*^9}, {3.678095978175391*^9, 3.6780960120591097`*^9}, {
   3.678096058570084*^9, 3.6780960634875927`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"RpToTrans", "[", 
      RowBox[{"R_", ",", "p_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "rotation", " ", "matrix", " ", "R", " ", "and", " ", 
      "position", " ", "p", " ", "and", " ", "returns", " ", "corresponding", 
      " ", "homogeneous", " ", "transformation", " ", "matrix", " ", "T", " ",
       "SE", 
      RowBox[{"(", "3", ")"}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "T"}], 
     " ", "=", " ", 
     RowBox[{
      RowBox[{"RpToTrans", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", 
            RowBox[{"-", "1"}]}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "}"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"1", ",", "2", ",", "5"}], "}"}], "}"}], 
         "\[Transpose]"}]}], "]"}], "//", 
      RowBox[{"MatrixForm", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"1", "0", "0", "1"},
            {"0", "0", 
             RowBox[{"-", "1"}], "2"},
            {"0", "1", "0", "5"},
            {"0", "0", "0", "1"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"RpToTrans", "[", 
     RowBox[{"R_", ",", "p_"}], "]"}], ":=", 
    RowBox[{"ArrayFlatten", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"R", ",", "p"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1"}], "}"}]}], "}"}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675595275623045*^9, 
   3.6755952858059273`*^9}, {3.675595499994834*^9, 3.675595501887823*^9}, {
   3.675595686331749*^9, 3.6755956966706448`*^9}, {3.6755964730817327`*^9, 
   3.6755964735643463`*^9}, 3.6780960195594473`*^9, {3.67809610144601*^9, 
   3.678096106509063*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"TransToRp", "[", "T_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "transformation", " ", "matrix", " ", "T", " ", "SE", 
      RowBox[{"(", "3", ")"}], " ", "and", " ", "returns", " ", "R", " ", 
      "the", " ", "corresponding", " ", "rotation", " ", "matrix", " ", "and",
       " ", "p", " ", "the", " ", "corresponding", " ", "position", " ", 
      RowBox[{"vector", ".", " ", "The"}], " ", "first", " ", "element", " ", 
      "of", " ", "the", " ", "output", " ", "is", " ", "the", " ", "rotation",
       " ", "matrix", " ", "and", " ", "the", " ", "second", " ", "is", " ", 
      "the", " ", "position", " ", 
      RowBox[{"vector", "."}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", 
      RowBox[{"{", 
       RowBox[{"R", ",", "p"}], "}"}]}], " ", "=", " ", 
     RowBox[{
      RowBox[{"TransToRp", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0", ",", "3"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}], "]"}], 
      "\[IndentingNewLine]", 
      RowBox[{"Output", ":", " ", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{"0", ",", "0", ",", 
             RowBox[{"-", "1"}]}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", "0", "}"}], ",", 
           RowBox[{"{", "0", "}"}], ",", 
           RowBox[{"{", "3", "}"}]}], "}"}]}], "}"}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"TransToRp", "[", "T_", "]"}], ":=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"T", "[", 
       RowBox[{"[", 
        RowBox[{
         RowBox[{"1", ";;", "3"}], ",", 
         RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], ",", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"T", "[", 
         RowBox[{"[", 
          RowBox[{
           RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "}"}], 
       "\[Transpose]"}]}], "}"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.6755959567829027`*^9, 
   3.675595965979847*^9}, {3.67559603247852*^9, 3.675596033012048*^9}, {
   3.675596680132864*^9, 3.6755966912919407`*^9}, {3.678096115211194*^9, 
   3.6780961185043497`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"TransInv", "[", "T_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "T", " ", "a", " ", "transformation", " ", "matrix", " ", 
      "and", " ", "returns", " ", "its", " ", 
      RowBox[{"inverse", ".", "\[IndentingNewLine]", "Uses"}], " ", "the", 
      " ", "structure", " ", "of", " ", "transformation", " ", "matrices", 
      " ", "to", " ", "avoid", " ", "taking", " ", "a", " ", "matrix", " ", 
      "inverse"}]}], ",", 
    RowBox[{
     RowBox[{"for", " ", 
      RowBox[{"efficiency", "."}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "invT"}], " ",
       "=", " ", 
      RowBox[{
       RowBox[{"TransInv", "[", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", 
            RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", "3"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}], "]"}],
        "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", " ", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1", "0", "0", "0"},
             {"0", "0", "1", 
              RowBox[{"-", "3"}]},
             {"0", 
              RowBox[{"-", "1"}], "0", "0"},
             {"0", "0", "0", "1"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"TransInv", "[", "T_", "]"}], ":=", "\[IndentingNewLine]", 
    RowBox[{"ArrayFlatten", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{
          RowBox[{"T", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"1", ";;", "3"}], ",", 
             RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "\[Transpose]"}], ",", 
         
         RowBox[{"-", 
          RowBox[{
           RowBox[{
            RowBox[{"T", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", 
               RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "\[Transpose]"}], 
           ".", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{"T", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "}"}], 
            "\[Transpose]"}]}]}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1"}], "}"}]}], "}"}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675596758956827*^9, 
   3.675596816647419*^9}, {3.6755968603462763`*^9, 3.675596904061962*^9}, {
   3.675596971182308*^9, 3.675596988729371*^9}, {3.6780961229623423`*^9, 
   3.6780961264717283`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"VecTose3", "[", "V_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "a", " ", "6"}], "-", 
      RowBox[{"vector", " ", 
       RowBox[{"(", 
        RowBox[{"representing", " ", "a", " ", "spatial", " ", "velocity"}], 
        ")"}], " ", "and", " ", "returns", " ", "the", " ", "corresponding", 
       " ", "4", "x4", " ", "se", 
       RowBox[{"(", "3", ")"}], " ", "matrix"}]}]}], "\[IndentingNewLine]", 
    ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "se3mat"}], 
     " ", "=", " ", 
     RowBox[{
      RowBox[{"VecTose3", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"{", 
          RowBox[{"1", ",", "2", ",", "3", ",", "4", ",", "5", ",", "6"}], 
          "}"}], "}"}], "\[Transpose]"}], "]"}], "//", 
      RowBox[{"MatrixForm", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"0", 
             RowBox[{"-", "3"}], "2", "4"},
            {"3", "0", 
             RowBox[{"-", "1"}], "5"},
            {
             RowBox[{"-", "2"}], "1", "0", "6"},
            {"0", "0", "0", "0"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"VecTose3", "[", "V_", "]"}], ":=", 
    RowBox[{"ArrayFlatten", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"VecToso3", "[", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"V", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "}"}], 
           "\[Transpose]"}], "]"}], ",", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"V", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "}"}], 
          "\[Transpose]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0"}], "}"}]}], "}"}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675597080161522*^9, 
   3.675597119970598*^9}, {3.675597158104844*^9, 3.675597160162319*^9}, {
   3.675597211069208*^9, 3.675597216985806*^9}, {3.678096131049049*^9, 
   3.678096133158791*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"se3ToVec", "[", "se3mat_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "se3mat", " ", "a", " ", "4", "x4", " ", "se", 
       RowBox[{"(", "3", ")"}], " ", "matrix", " ", "and", " ", "returns", 
       " ", "the", " ", "corresponding", " ", "6"}], "-", 
      RowBox[{"vector", " ", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"representing", " ", "spatial", " ", "velocity"}], ")"}], 
        "."}]}]}]}], "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "V"}], 
     " ", "=", " ", 
     RowBox[{
      RowBox[{"se3ToVec", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "3"}], ",", "2", ",", "4"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"3", ",", "0", ",", 
           RowBox[{"-", "1"}], ",", "5"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", "2"}], ",", "1", ",", "0", ",", "6"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}], "]"}], 
      "\[IndentingNewLine]", 
      RowBox[{"Output", ":", " ", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", "1", "}"}], ",", 
         RowBox[{"{", "2", "}"}], ",", 
         RowBox[{"{", "3", "}"}], ",", 
         RowBox[{"{", "4", "}"}], ",", 
         RowBox[{"{", "5", "}"}], ",", 
         RowBox[{"{", "6", "}"}]}], "}"}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"se3ToVec", "[", "se3mat_", "]"}], ":=", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"3", ",", "2"}], "]"}], "]"}], ",", 
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
        RowBox[{"se3mat", "[", 
         RowBox[{"[", 
          RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}], "}"}], 
     "\[Transpose]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.6755973505611753`*^9, 
   3.675597361941107*^9}, {3.675597420571553*^9, 3.675597428321558*^9}, {
   3.6755976369085283`*^9, 3.675597642481595*^9}, {3.6755978045694723`*^9, 
   3.675597816797654*^9}, {3.6780961362188787`*^9, 3.678096138047097*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"Adjoint", "[", "T_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "T", " ", "a", " ", "transformation", " ", "matrix", " ", 
      "SE3", " ", "and", " ", "returns", " ", "the", " ", "corresponding", 
      " ", "6", "x6", " ", "adjoint", " ", 
      RowBox[{"representation", "[", "AdT", "]"}]}]}], "\[IndentingNewLine]", 
    ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "AdT"}], " ", 
     "=", " ", 
     RowBox[{
      RowBox[{"Adjoint", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0", ",", "3"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}], "]"}], 
      "//", 
      RowBox[{"MatrixForm", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"1", "0", "0", "0", "0", "0"},
            {"0", "0", 
             RowBox[{"-", "1"}], "0", "0", "0"},
            {"0", "1", "0", "0", "0", "0"},
            {"0", "0", "3", "1", "0", "0"},
            {"3", "0", "0", "0", "0", 
             RowBox[{"-", "1"}]},
            {"0", "0", "0", "0", "1", "0"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"Adjoint", "[", "T_", "]"}], ":=", 
    RowBox[{"ArrayFlatten", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"T", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"1", ";;", "3"}], ",", 
            RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], ",", "0"}], "}"}], ",", 
       
       RowBox[{"{", 
        RowBox[{
         RowBox[{
          RowBox[{"VecToso3", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{"T", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "}"}], 
            "\[Transpose]"}], "]"}], ".", 
          RowBox[{"T", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"1", ";;", "3"}], ",", 
             RowBox[{"1", ";;", "3"}]}], "]"}], "]"}]}], ",", 
         RowBox[{"T", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"1", ";;", "3"}], ",", 
            RowBox[{"1", ";;", "3"}]}], "]"}], "]"}]}], "}"}]}], "}"}], 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675597878325226*^9, 
   3.6755979146605263`*^9}, {3.6755980148620157`*^9, 3.675598030554899*^9}, {
   3.678096141013015*^9, 3.678096147753409*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"ScrewToAxis", "[", 
      RowBox[{"q_", ",", "s_", ",", "h_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
     "Takes", " ", "a", " ", "point", " ", "q", " ", "lying", " ", "on", " ", 
      "the", " ", "screw", " ", "axis"}]}], ",", 
    RowBox[{
    "a", " ", "unit", " ", "vector", " ", "s", " ", "in", " ", "the", " ", 
     "direction", " ", "of", " ", "the", " ", "screw", " ", "axis"}], ",", 
    RowBox[{
     RowBox[{
     "and", " ", "the", " ", "pitch", " ", "h", " ", "of", " ", "the", " ", 
      "screw", " ", "axis", " ", "and", " ", "returns", " ", "the", " ", 
      "corresponding", " ", "normalized", " ", "screw", " ", 
      RowBox[{"axis", "."}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "S"}],
       " ", "=", " ", 
      RowBox[{
       RowBox[{"ScrewToAxis", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"{", 
           RowBox[{"{", 
            RowBox[{"3", ",", "0", ",", "0"}], "}"}], "}"}], "\[Transpose]"}],
          ",", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"{", 
            RowBox[{"0", ",", "0", ",", "1"}], "}"}], "}"}], "\[Transpose]"}],
          ",", "2"}], "]"}], "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "0", "}"}], ",", 
          RowBox[{"{", "0", "}"}], ",", 
          RowBox[{"{", "1", "}"}], ",", 
          RowBox[{"{", "0", "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"-", "3"}], "}"}], ",", 
          RowBox[{"{", "2", "}"}]}], "}"}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"ScrewToAxis", "[", 
     RowBox[{"q_", ",", "s_", ",", "h_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", "S", "}"}], ",", 
      RowBox[{
       RowBox[{"If", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Norm", "[", "s", "]"}], "\[NotEqual]", "1"}], ",", 
         RowBox[{
         "S", "=", 
          "\"\<\!\(\*OverscriptBox[\(s\), \(^\)]\) is not a unit \
vector\>\""}], ",", 
         RowBox[{"If", "[", 
          RowBox[{
           RowBox[{"h", "\[NotEqual]", "Infinity"}], ",", 
           RowBox[{"S", "=", 
            RowBox[{"ArrayFlatten", "[", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{"{", "s", "}"}], ",", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{
                  RowBox[{"{", 
                   RowBox[{"Cross", "[", 
                    RowBox[{
                    RowBox[{"Flatten", "[", 
                    RowBox[{"-", "s"}], "]"}], ",", 
                    RowBox[{"Flatten", "[", "q", "]"}]}], "]"}], "}"}], 
                  "\[Transpose]"}], "+", 
                 RowBox[{"h", "*", "s"}]}], "}"}]}], "}"}], "]"}]}], ",", 
           RowBox[{"S", "=", 
            RowBox[{"ArrayFlatten", "[", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{"{", 
                RowBox[{
                 RowBox[{"{", 
                  RowBox[{"{", 
                   RowBox[{"0", ",", "0", ",", "0"}], "}"}], "}"}], 
                 "\[Transpose]"}], "}"}], ",", 
               RowBox[{"{", "s", "}"}]}], "}"}], "]"}]}]}], "]"}]}], "]"}], 
       ";", "\[IndentingNewLine]", "S"}]}], "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675598055508669*^9, 
   3.675598057083598*^9}, {3.675598169865116*^9, 3.6755982065219803`*^9}, {
   3.675598239676413*^9, 3.675598240245709*^9}, {3.675598295505789*^9, 
   3.675598303845595*^9}, 3.675598755218297*^9, {3.675601452088194*^9, 
   3.675601456989585*^9}, 3.678096151946823*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"AxisAng6", "[", "expc6_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Takes", " ", "a", " ", "6"}], "-", 
      RowBox[{
      "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", "for",
        " ", "rigid"}], "-", 
      RowBox[{
      "body", " ", "motion", " ", "S", "*", "theta", " ", "and", " ", 
       "returns", " ", "S", " ", "the", " ", "corresponding", " ", 
       "normalized", " ", "screw", " ", "axis"}]}]}], ",", 
    RowBox[{
     RowBox[{
     "and", " ", "theta", " ", "the", " ", "distance", " ", "traveled", " ", 
      RowBox[{"along", "/", "about"}], " ", 
      RowBox[{"S", ".", " ", "The"}], " ", "first", " ", "element", " ", "of",
       " ", "the", " ", "output", " ", "is", " ", "the", " ", "screw", " ", 
      "axis", " ", "S", " ", "and", " ", "the", " ", "second", " ", "is", " ",
       "the", " ", "distance", " ", 
      RowBox[{"theta", "."}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", 
       RowBox[{"{", 
        RowBox[{"S", ",", " ", "theta"}], "}"}]}], " ", "=", " ", 
      RowBox[{
       RowBox[{"AxisAng6", "[", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "1", "}"}], ",", 
          RowBox[{"{", "0", "}"}], ",", 
          RowBox[{"{", "0", "}"}], ",", 
          RowBox[{"{", "1", "}"}], ",", 
          RowBox[{"{", "2", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "}"}], "]"}], "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{
            RowBox[{"{", "1", "}"}], ",", 
            RowBox[{"{", "0", "}"}], ",", 
            RowBox[{"{", "0", "}"}], ",", 
            RowBox[{"{", "1", "}"}], ",", 
            RowBox[{"{", "2", "}"}], ",", 
            RowBox[{"{", "3", "}"}]}], "}"}], ",", "1"}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"AxisAng6", "[", "expc6_", "]"}], ":=", 
    RowBox[{"If", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{"expc6", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "}"}], 
        "\[Transpose]"}], "\[Equal]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], "}"}], "\[Transpose]"}]}],
       ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"expc6", "/", 
         RowBox[{"Norm", "[", 
          RowBox[{"expc6", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "]"}]}], ",", 
        RowBox[{"Norm", "[", 
         RowBox[{"expc6", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "]"}]}], 
       "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"expc6", "/", 
         RowBox[{"Norm", "[", 
          RowBox[{"expc6", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "]"}]}], ",", 
        RowBox[{"Norm", "[", 
         RowBox[{"expc6", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "]"}]}], 
       "}"}]}], "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.6755985001383038`*^9, 
   3.675598581679422*^9}, {3.675598613345693*^9, 3.675598641956484*^9}, {
   3.675598705465678*^9, 3.675598735438822*^9}, 3.678096156180605*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{"MatrixExp6", "[", "expc6_", "]"}], ":", "\[IndentingNewLine]", 
      
      RowBox[{
       RowBox[{"Takes", " ", "a", " ", "6"}], "-", 
       RowBox[{
       "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", 
        RowBox[{"(", 
         RowBox[{"S", "*", "theta"}], ")"}], " ", "and", " ", "returns", " ", 
        "a", " ", "T", " ", "matrix", " ", "SE", 
        RowBox[{"(", "3", ")"}], " ", "that", " ", "is", " ", "achieved", " ",
         "by", " ", "traveling", " ", 
        RowBox[{"along", "/", "about"}], " ", "the", " ", "screw", " ", 
        "axis", " ", "S", " ", "for", " ", "a", " ", "distance", " ", "theta",
         " ", "from", " ", "an", " ", "initial", " ", "configuration", " ", 
        "T"}]}]}], "=", 
     RowBox[{
      RowBox[{"I", "\n", "\[IndentingNewLine]", "Rodriguez", " ", "R"}], "=", 
      
      RowBox[{"I", "+", 
       RowBox[{"sin", 
        RowBox[{"(", "theta", ")"}], "*", "omg"}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"1", "-", 
          RowBox[{"cos", 
           RowBox[{"(", "theta", ")"}]}]}], ")"}], "*", 
        RowBox[{"omg", "^", "2"}]}]}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "T"}], 
     " ", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"MatrixExp6", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "1.5707963267948966", ",", "0.0", ",", "0.0", ",", "0.0", ",", 
            "2.3561944901923448", ",", "2.3561944901923457"}], "}"}], "}"}], 
         "\[Transpose]"}], "]"}], "//", "MatrixForm"}], "//", 
      RowBox[{"N", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"1.", "0.", "0.", "0."},
            {"0.", "0.", 
             RowBox[{"-", "1."}], "0."},
            {"0.", "1.", "0.", "3."},
            {"0.", "0.", "0.", "1."}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"MatrixExp6", "[", "expc6_", "]"}], ":=", "\[IndentingNewLine]", 
    RowBox[{"If", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"expc6", "[", 
         RowBox[{"[", "1", "]"}], "]"}], " ", "\[Equal]", " ", 
        RowBox[{"{", "0", "}"}]}], "&&", " ", 
       RowBox[{
        RowBox[{"expc6", "[", 
         RowBox[{"[", "2", "]"}], "]"}], " ", "\[Equal]", " ", 
        RowBox[{"{", "0", "}"}]}], " ", "&&", " ", 
       RowBox[{
        RowBox[{"expc6", "[", 
         RowBox[{"[", "3", "]"}], "]"}], " ", "\[Equal]", " ", 
        RowBox[{"{", "0", "}"}]}]}], "  ", ",", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", 
          RowBox[{
           RowBox[{"expc6", "[", 
            RowBox[{"[", "4", "]"}], "]"}], "[", 
           RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", 
          RowBox[{
           RowBox[{"expc6", "[", 
            RowBox[{"[", "5", "]"}], "]"}], "[", 
           RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", 
          RowBox[{
           RowBox[{"expc6", "[", 
            RowBox[{"[", "6", "]"}], "]"}], "[", 
           RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}], ",", 
      " ", 
      RowBox[{"ArrayFlatten", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"MatrixExp3", "[", 
            RowBox[{
             RowBox[{"(", 
              RowBox[{
               RowBox[{"{", 
                RowBox[{
                 RowBox[{
                  RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                  RowBox[{"[", "1", "]"}], "]"}], "[", 
                 RowBox[{"[", 
                  RowBox[{
                   RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "}"}], 
               "\[Transpose]"}], ")"}], "*", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
               RowBox[{"[", "2", "]"}], "]"}], ")"}]}], "]"}], ",", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"IdentityMatrix", "[", "3", "]"}], "*", 
               RowBox[{
                RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                RowBox[{"[", "2", "]"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"(", 
                RowBox[{"1", "-", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                   RowBox[{"[", "2", "]"}], "]"}], "]"}]}], ")"}], "*", 
               RowBox[{"VecToso3", "[", 
                RowBox[{
                 RowBox[{"{", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                    RowBox[{"[", "1", "]"}], "]"}], "[", 
                   RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "}"}],
                  "\[Transpose]"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"(", 
                RowBox[{
                 RowBox[{
                  RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                  RowBox[{"[", "2", "]"}], "]"}], "-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                   RowBox[{"[", "2", "]"}], "]"}], "]"}]}], ")"}], "*", 
               RowBox[{"MatrixPower", "[", 
                RowBox[{
                 RowBox[{"VecToso3", "[", 
                  RowBox[{
                   RowBox[{"{", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                    RowBox[{"[", "1", "]"}], "]"}], "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "}"}],
                    "\[Transpose]"}], "]"}], ",", "2"}], "]"}]}]}], ")"}], 
            ".", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{
               RowBox[{
                RowBox[{"AxisAng6", "[", "expc6", "]"}], "[", 
                RowBox[{"[", "1", "]"}], "]"}], "[", 
               RowBox[{"[", 
                RowBox[{
                 RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "}"}], 
             "\[Transpose]"}]}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1"}], "}"}]}], "}"}], "]"}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.6755988080579853`*^9, 
   3.675598809147863*^9}, {3.67559891787045*^9, 3.675598943668881*^9}, {
   3.675599076607477*^9, 3.675599086502984*^9}, {3.675599163344201*^9, 
   3.675599246140224*^9}, {3.675599495513714*^9, 3.675599501889613*^9}, {
   3.6756003985310173`*^9, 3.67560041869237*^9}, {3.675600613139152*^9, 
   3.675600616115411*^9}, {3.675600652001363*^9, 3.6756006755021143`*^9}, {
   3.675600735634182*^9, 3.6756007488548737`*^9}, {3.67560092599559*^9, 
   3.675600966443502*^9}, 3.67561176087851*^9, {3.675614302769956*^9, 
   3.675614306885004*^9}, {3.675614423023447*^9, 3.675614423525736*^9}, 
   3.678096161809099*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"MatrixLog6", "[", "T_", "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Takes", " ", "a", " ", "transformation", " ", "matrix", " ", "T", " ", 
       "SE", 
       RowBox[{"(", "3", ")"}], " ", "and", " ", "returns", " ", "the", " ", 
       "corresponding", " ", "6"}], "-", 
      RowBox[{
      "vector", " ", "of", " ", "exponential", " ", "coordinates", " ", "S", 
       "*", 
       RowBox[{"theta", "."}]}]}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "expc6"}], " ",
      "=", " ", 
     RowBox[{
      RowBox[{"MatrixLog6", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0", ",", "3"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}], "]"}], 
      "//", 
      RowBox[{"N", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "1.5707963267948966", "}"}], ",", 
          RowBox[{"{", "0.", "}"}], ",", 
          RowBox[{"{", "0.", "}"}], ",", 
          RowBox[{"{", "0.", "}"}], ",", 
          RowBox[{"{", "2.356194490192345", "}"}], ",", 
          RowBox[{"{", "2.356194490192345", "}"}]}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{"MatrixLog6", "[", "T_", "]"}], ":=", 
     RowBox[{"Module", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"expc6", ",", "theta"}], "}"}], ",", 
       RowBox[{
        RowBox[{"theta", "=", 
         RowBox[{"ArcCos", "[", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"Tr", "[", 
              RowBox[{"T", "[", 
               RowBox[{"[", 
                RowBox[{
                 RowBox[{"1", ";;", "3"}], ",", 
                 RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "]"}], "-", "1"}], 
            ")"}], "/", "2"}], "]"}]}], ";", 
        RowBox[{"Which", "[", 
         RowBox[{
          RowBox[{
           RowBox[{
            RowBox[{"T", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", 
               RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "\[Equal]", 
            RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "&&", 
           RowBox[{
            RowBox[{"Norm", "[", 
             RowBox[{"T", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "]"}], 
            "\[NotEqual]", "0"}]}], ",", 
          RowBox[{"expc6", "=", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{"Flatten", "[", 
              RowBox[{"{", 
               RowBox[{"0", ",", "0", ",", "0", ",", 
                RowBox[{"T", "[", 
                 RowBox[{"[", 
                  RowBox[{
                   RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}]}], 
               "}"}], "]"}], "}"}], "\[Transpose]"}]}], ",", 
          RowBox[{
           RowBox[{
            RowBox[{"T", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", 
               RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "\[Equal]", 
            RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "&&", 
           RowBox[{
            RowBox[{"Norm", "[", 
             RowBox[{"T", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "]"}], 
            "\[Equal]", "0"}]}], ",", 
          RowBox[{"expc6", "=", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{"{", 
              RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}],
               "}"}], "}"}], "\[Transpose]"}]}], ",", 
          RowBox[{
           RowBox[{"Tr", "[", 
            RowBox[{"T", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", 
               RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "]"}], "\[Equal]", 
           RowBox[{"-", "1"}]}], ",", 
          RowBox[{"Which", "[", 
           RowBox[{
            RowBox[{
             RowBox[{"1", "+", 
              RowBox[{"T", "[", 
               RowBox[{"[", 
                RowBox[{"3", ",", "3"}], "]"}], "]"}]}], "\[NotEqual]", " ", 
             "0"}], ",", 
            RowBox[{"expc6", "=", 
             RowBox[{
              RowBox[{
               RowBox[{"{", 
                RowBox[{"Join", "[", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                    "*", 
                   RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}]}], "}"}], "}"}], 
                    "[", 
                    RowBox[{"[", "1", "]"}], "]"}]}], ",", 
                  RowBox[{
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "*", 
                    RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}]}], "+", 
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"Cot", "[", 
                    RowBox[{"Pi", "/", "2"}], "]"}]}]}], ")"}], "*", 
                    RowBox[{"MatrixPower", "[", 
                    RowBox[{
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "3"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "3"}], "]"}], "]"}]}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}], ",", "2"}], "]"}]}]}], ")"}], 
                   ".", 
                   RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}]}]}], 
                 "]"}], "}"}], "\[Transpose]"}], "*", "Pi"}]}], ",", 
            RowBox[{
             RowBox[{"1", "+", 
              RowBox[{"T", "[", 
               RowBox[{"[", 
                RowBox[{"2", ",", "2"}], "]"}], "]"}]}], "\[NotEqual]", "0"}],
             ",", 
            RowBox[{"expc6", "=", 
             RowBox[{
              RowBox[{
               RowBox[{"{", 
                RowBox[{"Join", "[", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                    "*", 
                   RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "2"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "2"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "[", 
                    RowBox[{"[", "1", "]"}], "]"}]}], ",", 
                  RowBox[{
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "*", 
                    RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "2"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "2"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}]}], "+", 
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"Cot", "[", 
                    RowBox[{"Pi", "/", "2"}], "]"}]}]}], ")"}], "*", 
                    RowBox[{"MatrixPower", "[", 
                    RowBox[{
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "2"}], "]"}], "]"}], ",", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "2"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "2"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}], ",", "2"}], "]"}]}]}], ")"}], 
                   ".", 
                   RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}]}]}], 
                 "]"}], "}"}], "\[Transpose]"}], "*", "Pi"}]}], ",", 
            RowBox[{
             RowBox[{"1", "+", 
              RowBox[{"T", "[", 
               RowBox[{"[", 
                RowBox[{"1", ",", "1"}], "]"}], "]"}]}], "\[NotEqual]", " ", 
             "0"}], ",", 
            RowBox[{"expc6", "=", 
             RowBox[{
              RowBox[{
               RowBox[{"{", 
                RowBox[{"Join", "[", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                    "*", 
                   RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "1"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "[", 
                    RowBox[{"[", "1", "]"}], "]"}]}], ",", 
                  RowBox[{
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "*", 
                    RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "1"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}]}], "+", 
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"1", "/", "Pi"}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"Cot", "[", 
                    RowBox[{"Pi", "/", "2"}], "]"}]}]}], ")"}], "*", 
                    RowBox[{"MatrixPower", "[", 
                    RowBox[{
                    RowBox[{"VecToso3", "[", 
                    RowBox[{
                    RowBox[{"1", "/", 
                    RowBox[{"Sqrt", "[", 
                    RowBox[{"2", 
                    RowBox[{"(", 
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ")"}]}], "]"}]}],
                     "*", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"1", "+", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"1", ",", "1"}], "]"}], "]"}]}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{"3", ",", "1"}], "]"}], "]"}]}], "}"}], "}"}], 
                    "\[Transpose]"}]}], "]"}], ",", "2"}], "]"}]}]}], ")"}], 
                   ".", 
                   RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}]}]}], 
                 "]"}], "}"}], "\[Transpose]"}], "*", "Pi"}]}]}], "]"}], ",", 
          "True", ",", 
          RowBox[{"expc6", "=", 
           RowBox[{
            RowBox[{
             RowBox[{"{", 
              RowBox[{"Join", "[", 
               RowBox[{
                RowBox[{
                 RowBox[{
                  RowBox[{"so3ToVec", "[", 
                   RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "-", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], 
                    "\[Transpose]"}]}], ")"}], "/", 
                    RowBox[{"(", 
                    RowBox[{"2", "*", 
                    RowBox[{"Sin", "[", "theta", "]"}]}], ")"}]}], "]"}], 
                  "\[Transpose]"}], "[", 
                 RowBox[{"[", "1", "]"}], "]"}], ",", 
                RowBox[{
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"1", "/", "theta"}], "*", 
                    RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "-", 
                   RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "-", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], 
                    "\[Transpose]"}]}], ")"}], "/", 
                    RowBox[{"(", 
                    RowBox[{"2", "*", 
                    RowBox[{"Sin", "[", "theta", "]"}]}], ")"}]}], ")"}]}], 
                   "+", 
                   RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"1", "/", "theta"}], "-", 
                    RowBox[{
                    RowBox[{"1", "/", "2"}], "*", 
                    RowBox[{"Cot", "[", 
                    RowBox[{"theta", "/", "2"}], "]"}]}]}], ")"}], "*", 
                    RowBox[{"MatrixPower", "[", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], "-", 
                    RowBox[{
                    RowBox[{"T", "[", 
                    RowBox[{"[", 
                    RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", 
                    RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], 
                    "\[Transpose]"}]}], ")"}], "/", 
                    RowBox[{"(", 
                    RowBox[{"2", "*", 
                    RowBox[{"Sin", "[", "theta", "]"}]}], ")"}]}], ",", "2"}],
                     "]"}]}]}], ")"}], ".", 
                 RowBox[{"T", "[", 
                  RowBox[{"[", 
                   RowBox[{
                    RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}]}]}], 
               "]"}], "}"}], "\[Transpose]"}], "*", "theta"}]}]}], "]"}], ";", 
        RowBox[{"Return", "[", "expc6", "]"}]}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675601244578272*^9, 
   3.675601274744042*^9}, {3.675601332061564*^9, 3.6756013660455017`*^9}, {
   3.675601414151766*^9, 3.675601425123197*^9}, {3.6756015530027637`*^9, 
   3.675601569614365*^9}, {3.6756016074254293`*^9, 3.6756016077890882`*^9}, {
   3.675601716305842*^9, 3.675601756140202*^9}, 3.678096166990847*^9}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  4: FORWARD KINEMATICS",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"FKinBody", "[", 
      RowBox[{"M_", ",", "Blist_", ",", "thetalist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "M"}], ":", 
     RowBox[{
      RowBox[{"the", " ", "home", " ", "configuration", " ", 
       RowBox[{"(", 
        RowBox[{"position", " ", "and", " ", "orientation"}], ")"}], " ", 
       "of", " ", "the", " ", "end"}], "-", "effector"}]}], ",", 
    RowBox[{"Blist", ":", 
     RowBox[{
      RowBox[{
      "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
       " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "frame", " ", "when", " ", "the", " ", "manipulator", 
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{
     RowBox[{"thetalist", ":", 
      RowBox[{
       RowBox[{
       "A", " ", "list", " ", "of", " ", "joint", " ", "coordinates", "\n", 
        "and", " ", "returns", " ", "T", " ", 
        RowBox[{"(", 
         RowBox[{"SE", 
          RowBox[{"(", "3", ")"}]}], ")"}], " ", "representing", " ", "the", 
        " ", "end"}], "-", 
       RowBox[{
       "effector", " ", "frame", " ", "when", " ", "the", " ", "joints", " ", 
        "are", " ", "at", " ", "the", " ", "specified", " ", "coordinates", 
        " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"i", ".", "t", ".", "o"}], " ", "Body", " ", "Frame"}], 
         ")"}]}]}]}], "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Inputs", ":", 
       "\[IndentingNewLine]", "M"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "6"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"-", "1"}], ",", "2"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Blist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", 
            RowBox[{"-", "1"}], ",", "2", ",", "0", ",", "0"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0.1"}], 
           "}"}], "}"}], "\[Transpose]"}]}], "}"}]}], ";", "\n", 
     RowBox[{"thetalist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"Pi", "/", "2.0"}], ")"}], ",", "3", ",", "Pi"}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"T", " ", "=", " ", 
      RowBox[{
       RowBox[{"FKinBody", "[", 
        RowBox[{"M", ",", "Blist", ",", "thetalist"}], "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", " ", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1.1102230246251565*^-16", "1.", "0.", 
              RowBox[{"-", "5."}]},
             {"1.", 
              RowBox[{"-", "1.1102230246251565*^-16"}], "0.", "4."},
             {"0.", "0.", 
              RowBox[{"-", "1."}], "1.6858407346410207"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"FKinBody", "[", 
     RowBox[{"M_", ",", "Blist_", ",", "thetalist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"i", ",", "j", ",", "T"}], "}"}], ",", 
      RowBox[{
       RowBox[{"i", "=", 
        RowBox[{"Length", "[", "Blist", "]"}]}], ";", 
       RowBox[{"T", "=", "M"}], ";", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"T", "=", 
          RowBox[{"T", ".", 
           RowBox[{"MatrixExp6", "[", 
            RowBox[{
             RowBox[{"Blist", "[", 
              RowBox[{"[", "j", "]"}], "]"}], "*", 
             RowBox[{"thetalist", "[", 
              RowBox[{"[", "j", "]"}], "]"}]}], "]"}]}]}], ",", 
         RowBox[{"{", 
          RowBox[{"j", ",", "i"}], "}"}]}], "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "T", "]"}]}]}], "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
   3.675531958735077*^9, 3.675532032779791*^9}, {3.6755321540719852`*^9, 
   3.675532155394449*^9}, 3.67559226134205*^9, {3.675596340559066*^9, 
   3.675596342576479*^9}, {3.675608043908893*^9, 3.67560805615449*^9}, {
   3.6756083091754017`*^9, 3.675608326044199*^9}, {3.6756089089651537`*^9, 
   3.6756089115657463`*^9}, {3.675609347333695*^9, 3.6756093476969*^9}, {
   3.675614529395809*^9, 3.675614539340641*^9}, {3.675614717287609*^9, 
   3.6756147189544277`*^9}, {3.675614753634843*^9, 3.675614844854802*^9}, {
   3.6756150166027203`*^9, 3.6756150520677147`*^9}, {3.6756150854940557`*^9, 
   3.675615119635448*^9}, {3.67561583526125*^9, 3.675615944525145*^9}, {
   3.675615986252219*^9, 3.67561600346618*^9}, {3.676118088970602*^9, 
   3.676118088974634*^9}, {3.676119594877483*^9, 3.676119639850988*^9}, 
   3.6780961875824127`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"FKinSpace", "[", 
      RowBox[{"M_", ",", "Slist_", ",", "thetalist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "M"}], ":", 
     RowBox[{
      RowBox[{"the", " ", "home", " ", "configuration", " ", 
       RowBox[{"(", 
        RowBox[{"position", " ", "and", " ", "orientation"}], ")"}], " ", 
       "of", " ", "the", " ", "end"}], "-", "effector"}]}], ",", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
      " ", "space", " ", "frame", " ", "when", " ", "the", " ", "manipulator",
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"thetalist", ":", 
      RowBox[{
       RowBox[{
       "A", " ", "list", " ", "of", " ", "joint", " ", "coordinates", "\n", 
        "and", " ", "returns", " ", "T", " ", 
        RowBox[{"(", 
         RowBox[{"SE", 
          RowBox[{"(", "3", ")"}]}], ")"}], " ", "representing", " ", "the", 
        " ", "end"}], "-", 
       RowBox[{
       "effector", " ", "frame", " ", "when", " ", "the", " ", "joints", " ", 
        "are", " ", "at", " ", "the", " ", "specified", " ", "coordinates", 
        " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"i", ".", "t", ".", "o"}], " ", "Fixed", " ", "Space", " ", 
          "Frame"}], ")"}]}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Inputs", ":", 
       "\[IndentingNewLine]", "M"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "6"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"-", "1"}], ",", "2"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "4", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", 
            RowBox[{"-", "1"}], ",", 
            RowBox[{"-", "6"}], ",", "0", ",", 
            RowBox[{"-", "0.1"}]}], "}"}], "}"}], "\[Transpose]"}]}], "}"}]}],
      ";", "\n", 
     RowBox[{"thetalist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"Pi", "/", "2.0"}], ")"}], ",", "3", ",", "Pi"}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"T", " ", "=", " ", 
      RowBox[{
       RowBox[{"FKinSpace", "[", 
        RowBox[{"M", ",", "Slist", ",", "thetalist"}], "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", " ", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1.1102230246251565`*^-16", "1.`", "0.`", 
              RowBox[{"-", "5.`"}]},
             {"1.`", 
              RowBox[{"-", "1.1102230246251565`*^-16"}], "0.`", 
              "4.000000000000001`"},
             {"0.`", "0.`", 
              RowBox[{"-", "1.`"}], "1.6858407346410207`"},
             {"0.`", "0.`", "0.`", "1.`"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"FKinSpace", "[", 
     RowBox[{"M_", ",", "Slist_", ",", "thetalist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"i", ",", "j", ",", "T"}], "}"}], ",", 
      RowBox[{
       RowBox[{"i", "=", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", 
       RowBox[{"T", "=", "M"}], ";", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"T", "=", 
          RowBox[{
           RowBox[{"MatrixExp6", "[", 
            RowBox[{
             RowBox[{"Slist", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "j", "+", "1"}], "]"}], "]"}], "*", 
             RowBox[{"thetalist", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "j", "+", "1"}], "]"}], "]"}]}], "]"}], ".", 
           "T"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"j", ",", "i"}], "}"}]}], "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "T", "]"}]}]}], "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.67561602324156*^9, 3.6756160562747183`*^9}, {
   3.675616098480197*^9, 3.6756162304999313`*^9}, {3.675616267860519*^9, 
   3.6756162750402412`*^9}, {3.675616305671249*^9, 3.675616309582301*^9}, {
   3.6756163906900806`*^9, 3.675616392204427*^9}, {3.675616509110962*^9, 
   3.675616528559065*^9}, {3.676118088983922*^9, 3.676118088989098*^9}, {
   3.6774897363386793`*^9, 3.6774897422094803`*^9}, {3.677489976352415*^9, 
   3.677489976705158*^9}, 3.6780961963052464`*^9}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  5: VELOCITY KINEMATICS AND STATICS",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}, {3.6756821024917507`*^9, 3.6756821283850946`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"JacobianBody", "[", 
      RowBox[{"Blist_", ",", "thetalist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Blist"}], ":", " ", 
     RowBox[{
      RowBox[{
      "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
       " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "frame", " ", "when", " ", "the", " ", "manipulator", 
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{
     RowBox[{"thetalist", ":", 
      RowBox[{
      "A", " ", "list", " ", "of", " ", "joint", " ", "coordinates", "\n", 
       "and", " ", "returns", " ", "the", " ", "corresponding", " ", "body", 
       " ", "Jacobian", " ", 
       RowBox[{"(", 
        RowBox[{"6", "xn", " ", "real", " ", "numbers"}], ")"}]}]}], 
     "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Blist"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "0", ",", "0", ",", "1", ",", "0", ",", "0.2", ",", "0.2"}], "}"}],
           "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"1", ",", "0", ",", "0", ",", "2", ",", "0", ",", "3"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "2", ",", "1"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "1", ",", "0", ",", "0", ",", "0.2", ",", "0.3", ",", "0.4"}], 
           "}"}], "}"}], "\[Transpose]"}]}], "}"}]}], ";", "\n", 
     RowBox[{"thetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.2", ",", "1.1", ",", "0.1", ",", "1.2"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"Jb", " ", "=", " ", 
      RowBox[{
       RowBox[{"JacobianBody", "[", 
        RowBox[{"Blist", ",", " ", "thetalist"}], "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {
              RowBox[{"-", "0.04528405057966491"}], "0.9950041652780258", 
              "0.", "1"},
             {"0.7435931265563965", "0.09304864640049498", 
              "0.3623577544766736", "0"},
             {
              RowBox[{"-", "0.6670971570177624"}], "0.03617541267787882", 
              RowBox[{"-", "0.9320390859672263"}], "0"},
             {"2.3258604714595155", "1.668090004953633", "0.5641083080438885",
               "0.2"},
             {
              RowBox[{"-", "1.4432116718196153"}], "2.945612749911765", 
              "1.4330652142884392", "0.3"},
             {
              RowBox[{"-", "2.066395648760265"}], "1.8288172246233696", 
              RowBox[{"-", "1.5886862785321807"}], "0.4"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"JacobianBody", "[", 
     RowBox[{"Blist_", ",", "thetalist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"Jb", ",", "n", ",", "Ti", ",", "Vs"}], "}"}], ",", 
      RowBox[{
       RowBox[{"n", "=", 
        RowBox[{"Length", "[", "Blist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Ti", " ", "=", " ", 
           RowBox[{"IdentityMatrix", "[", "4", "]"}]}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Do", "[", 
           RowBox[{
            RowBox[{"Ti", "=", 
             RowBox[{
              RowBox[{"MatrixExp6", "[", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Blist", "[", 
                  RowBox[{"[", "jj", "]"}], "]"}]}], "*", 
                RowBox[{"thetalist", "[", 
                 RowBox[{"[", "jj", "]"}], "]"}]}], "]"}], ".", "Ti"}]}], ",", 
            RowBox[{"{", 
             RowBox[{"jj", ",", 
              RowBox[{"ii", "+", "1"}], ",", "n"}], "}"}]}], "]"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Vs", " ", "=", " ", 
           RowBox[{
            RowBox[{"Adjoint", "[", "Ti", "]"}], ".", 
            RowBox[{"Blist", "[", 
             RowBox[{"[", "ii", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
          
          RowBox[{"If", "[", 
           RowBox[{
            RowBox[{"ii", " ", "\[Equal]", " ", "1"}], ",", 
            RowBox[{"Jb", " ", "=", " ", "Vs"}], ",", "\[IndentingNewLine]", 
            RowBox[{"Jb", " ", "=", 
             RowBox[{"ArrayFlatten", "[", 
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{"Jb", ",", "Vs"}], "}"}], "}"}], "]"}]}]}], "]"}], 
          ";"}], "\[IndentingNewLine]", ",", 
         RowBox[{"{", 
          RowBox[{"ii", ",", "1", ",", 
           RowBox[{"n", "-", "1"}]}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Vs", " ", "=", " ", 
        RowBox[{"Blist", "[", 
         RowBox[{"[", "n", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Jb", " ", "=", " ", 
        RowBox[{"ArrayFlatten", "[", 
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"Jb", ",", "Vs"}], "}"}], "}"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "Jb", "]"}], ";"}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675682824450754*^9, 
   3.6756828836706333`*^9}, {3.6756829858535852`*^9, 3.675683093476144*^9}, {
   3.675683163431617*^9, 3.675683301926485*^9}, {3.6756834458193197`*^9, 
   3.675683520674423*^9}, {3.675683569012762*^9, 3.675683584065415*^9}, 
   3.675683635500771*^9, {3.675684508273862*^9, 3.675684520687911*^9}, {
   3.676118088998353*^9, 3.6761180890027103`*^9}, {3.6780962114132557`*^9, 
   3.6780962222523317`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"JacobianSpace", "[", 
      RowBox[{"Slist_", ",", "thetalist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Slist"}], ":", 
     RowBox[{
     "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
      " ", "space", " ", "frame", " ", "when", " ", "the", " ", "manipulator",
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"thetalist", ":", 
      RowBox[{
      "A", " ", "list", " ", "of", " ", "joint", " ", "coordinates", "\n", 
       "and", " ", "returns", " ", "the", " ", "corresponding", " ", "space", 
       " ", "Jacobian", " ", 
       RowBox[{"(", 
        RowBox[{"6", "xn", " ", "real", " ", "numbers"}], ")"}]}]}], 
     "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Slist"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "0", ",", "0", ",", "1", ",", "0", ",", "0.2", ",", "0.2"}], "}"}],
           "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"1", ",", "0", ",", "0", ",", "2", ",", "0", ",", "3"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "2", ",", "1"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", " ", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "1", ",", "0", ",", "0", ",", "0.2", ",", "0.3", ",", "0.4"}], 
           "}"}], "}"}], "\[Transpose]"}]}], "}"}]}], ";", "\n", 
     RowBox[{"thetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.2", ",", "1.1", ",", "0.1", ",", "1.2"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"Js", " ", "=", " ", 
      RowBox[{
       RowBox[{"JacobianSpace", "[", 
        RowBox[{"Slist", ",", " ", "thetalist"}], "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", 
        RowBox[{"Ouput", ":", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0", "0.9800665778412416", 
              RowBox[{"-", "0.09011563789485476"}], "0.957494264730031"},
             {"0", "0.19866933079506122", "0.4445543984476258", 
              "0.28487556541794845"},
             {"1", "0.", "0.8912073600614354", 
              RowBox[{"-", "0.04528405057966491"}]},
             {"0", "1.9521863824506809", 
              RowBox[{"-", "2.216352156896298"}], 
              RowBox[{"-", "0.5116153729819477"}]},
             {"0.2", "0.4365413247037721", 
              RowBox[{"-", "2.437125727653321"}], "2.7753571339551537"},
             {"0.2", "2.960266133840988", "3.2357306532803083", 
              "2.22512443353574"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"JacobianSpace", "[", 
     RowBox[{"Slist_", ",", "thetalist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"Js", ",", "n", ",", "Ti", ",", "Vs"}], "}"}], ",", 
      RowBox[{
       RowBox[{"n", "=", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Js", " ", "=", " ", 
        RowBox[{"Slist", "[", 
         RowBox[{"[", "1", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Ti", " ", "=", " ", 
           RowBox[{"IdentityMatrix", "[", "4", "]"}]}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Do", "[", 
           RowBox[{
            RowBox[{"Ti", "=", 
             RowBox[{
              RowBox[{"MatrixExp6", "[", 
               RowBox[{
                RowBox[{"Slist", "[", 
                 RowBox[{"[", "jj", "]"}], "]"}], "*", 
                RowBox[{"thetalist", "[", 
                 RowBox[{"[", "jj", "]"}], "]"}]}], "]"}], ".", "Ti"}]}], ",", 
            RowBox[{"{", 
             RowBox[{"jj", ",", 
              RowBox[{"ii", "-", "1"}], ",", "1", ",", 
              RowBox[{"-", "1"}]}], "}"}]}], "]"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Vs", " ", "=", " ", 
           RowBox[{
            RowBox[{"Adjoint", "[", "Ti", "]"}], ".", 
            RowBox[{"Slist", "[", 
             RowBox[{"[", "ii", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
          
          RowBox[{"Js", " ", "=", 
           RowBox[{"ArrayFlatten", "[", 
            RowBox[{"{", 
             RowBox[{"{", 
              RowBox[{"Js", ",", "Vs"}], "}"}], "}"}], "]"}]}], ";"}], 
         "\[IndentingNewLine]", ",", 
         RowBox[{"{", 
          RowBox[{"ii", ",", "2", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "Js", "]"}], ";"}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675684467101232*^9, 
   3.6756844876690702`*^9}, 3.675685101672298*^9, {3.675685132865593*^9, 
   3.675685170606345*^9}, {3.6756852137594137`*^9, 3.675685253900947*^9}, {
   3.675685284425191*^9, 3.6756853403668823`*^9}, {3.6756853826789017`*^9, 
   3.675685417250636*^9}, {3.6761180890121613`*^9, 3.6761180890163*^9}, {
   3.6772374764222107`*^9, 3.677237478976756*^9}, 3.6774898698726873`*^9, {
   3.67748999177361*^9, 3.677489992267871*^9}, {3.678096227392668*^9, 
   3.6780962321466627`*^9}}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  6: INVERSE KINEMATICS",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}, {3.6756821024917507`*^9, 3.6756821283850946`*^9}, {
  3.6757648758196497`*^9, 3.675764892595064*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"IKinBody", "[", 
      RowBox[{
      "Blist_", ",", "M_", ",", "T_", ",", "thetalist0_", ",", "eomg_", ",", 
       "ev_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Blist"}], ":", 
     RowBox[{
      RowBox[{
      "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
       " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "frame", " ", "when", " ", "the", " ", "manipulator", 
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"M", ":", 
     RowBox[{
      RowBox[{
      "The", " ", "home", " ", "configuration", " ", "of", " ", "the", " ", 
       "end"}], "-", "effector"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"T", ":", 
     RowBox[{
      RowBox[{"The", " ", "desired", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration", " ", "T"}]}]}], ",", "\n", 
    RowBox[{"thetalist0", ":", 
     RowBox[{
     "An", " ", "initial", " ", "guess", " ", "of", " ", "joint", " ", 
      "angles", " ", "that", " ", "are", " ", "close", " ", "to", " ", 
      "satisfying", " ", "T"}]}], ",", "\n", 
    RowBox[{"eomg", ":", 
     RowBox[{
      RowBox[{
      "A", " ", "small", " ", "positive", " ", "tolerance", " ", "on", " ", 
       "the", " ", "end"}], "-", 
      RowBox[{"effector", " ", "orientation", " ", 
       RowBox[{"error", ".", " ", "The"}], " ", "returned", " ", "joint", " ",
        "angles", " ", "must", " ", "give", " ", "an", " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "orientation", " ", "error", " ", "less", " ", "than", 
       " ", "eomg"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"ev", ":", 
     RowBox[{
      RowBox[{
      "A", " ", "small", " ", "positive", " ", "tolerance", " ", "on", " ", 
       "the", " ", "end"}], "-", 
      RowBox[{"effector", " ", "linear", " ", "position", " ", 
       RowBox[{"error", ".", " ", "The"}], " ", "returned", " ", "joint", " ",
        "angles", " ", "must", " ", "give", " ", "an", " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "position", " ", "error", " ", "less", " ", "than", 
       " ", 
       RowBox[{
       "ev", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "The"}], " ", 
       "maximum", " ", "number", " ", "of", " ", "iterations", " ", "before", 
       " ", "the", " ", "algorithm", " ", "is", " ", "terminated", " ", "has",
        " ", "been", " ", "hardcoded", " ", "in", " ", "as", " ", "a", " ", 
       "variable", " ", "called", " ", 
       RowBox[{"maxiterations", ".", "It"}], " ", "is", " ", "set", " ", "to",
        " ", "20", " ", "at", " ", "the", " ", "start", " ", "of", " ", "the",
        " ", "function"}]}]}], ",", 
    RowBox[{"but", " ", "can", " ", "be", " ", "changed", " ", "if", " ", 
     RowBox[{
     "needed", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}],
      " ", 
     RowBox[{"thetalist", ":", 
      RowBox[{
      "Joint", " ", "angles", " ", "that", " ", "achieve", " ", "T", " ", 
       "within", " ", "the", " ", "specified", " ", "tolerances"}]}]}], ",", 
    RowBox[{
     RowBox[{"success", ":", 
      RowBox[{
       RowBox[{
       "A", " ", "logical", " ", "value", " ", "where", " ", "True", " ", 
        "means", " ", "that", " ", "the", " ", "function", " ", "found", " ", 
        "a", " ", "solution", " ", "and", " ", "False", "\n", "means", " ", 
        "that", " ", "it", " ", "ran", " ", "through", " ", "the", " ", "set",
         " ", "number", " ", "of", " ", "maximum", " ", "iterations", " ", 
        "without", " ", "finding", " ", "a", " ", "solution", "\n", "within", 
        " ", "the", " ", "tolerances", " ", "eomg", " ", "and", " ", 
        RowBox[{"ev", ".", "\[IndentingNewLine]", "Uses"}], " ", "an", " ", 
        "iterative", " ", "Newton"}], "-", 
       RowBox[{"Raphson", " ", "root"}], "-", 
       RowBox[{"finding", " ", "method"}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Exapmle", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Blist"}], " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "0", ",", "1", ",", "0", ",", "0.191", ",", "0", ",", "0.817"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0.095", ",", 
            RowBox[{"-", "0.817"}], ",", "0"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0.095", ",", 
            RowBox[{"-", "0.392"}], ",", "0"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
           "0", ",", "0", ",", "1", ",", "0.095", ",", "0", ",", "0"}], "}"}],
           "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", 
            RowBox[{"-", "1"}], ",", "0", ",", 
            RowBox[{"-", "0.082"}], ",", "0", ",", "0"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"M", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", 
          RowBox[{"-", "0.817"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"-", "1"}], ",", 
          RowBox[{"-", "0.191"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", 
          RowBox[{"-", "0.006"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"T", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", 
          RowBox[{"-", "0.6"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"-", "1"}], ",", ".1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"-", "1"}], ",", "0", ",", "0", ",", ".1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"thetalist0", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
       "}"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"eomg", "=", "0.01"}], ";", "\n", 
     RowBox[{"ev", "=", "0.001"}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"thetalist", ",", " ", "success"}], "}"}], " ", "=", " ", 
      RowBox[{
       RowBox[{"IKinBody", "[", 
        RowBox[{
        "Blist", ",", "M", ",", "T", ",", "thetalist0", ",", "eomg", ",", 
         "ev"}], "]"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Ouput", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{
            RowBox[{"-", "0.46921904513283463"}], ",", 
            RowBox[{"-", "0.8344762164215254"}], ",", "1.3952522291242646", 
            ",", 
            RowBox[{"-", "0.5610748581352074"}], ",", 
            RowBox[{"-", "0.4673132630155212"}], ",", 
            RowBox[{"-", "1.570563515310548"}]}], "}"}], ",", "True"}], 
         "}"}]}]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"IKinBody", "[", 
     RowBox[{
     "Blist_", ",", "M_", ",", "T_", ",", "thetalist0_", ",", "eomg_", ",", 
      "ev_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"i", ",", 
        RowBox[{"maxiterations", " ", "=", " ", "20"}], ",", "success", ",", 
        "\[Theta]i", ",", "thetalist", ",", "Tsb", ",", "Vb", ",", "Jb", ",", 
        "Jt"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"i", "=", "0"}], ";", "\[IndentingNewLine]", 
       RowBox[{"success", " ", "=", " ", "True"}], ";", "\[IndentingNewLine]", 
       RowBox[{"\[Theta]i", " ", "=", " ", "thetalist0"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"thetalist", " ", "=", " ", 
        RowBox[{"{", "\[Theta]i", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Tsb", " ", "=", " ", 
        RowBox[{"FKinBody", "[", 
         RowBox[{"M", ",", "Blist", ",", "thetalist0"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Vb", " ", "=", " ", 
        RowBox[{"MatrixLog6", "[", 
         RowBox[{
          RowBox[{"TransInv", "[", "Tsb", "]"}], ".", "T"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"While", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{"Norm", "[", 
            RowBox[{"Vb", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "]"}], ">",
            "eomg"}], " ", "||", " ", 
          RowBox[{
           RowBox[{"Norm", "[", 
            RowBox[{"Vb", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "]"}], ">",
            "ev"}]}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"If", "[", 
           RowBox[{
            RowBox[{"i", " ", "<", " ", "maxiterations"}], ",", " ", 
            RowBox[{
             RowBox[{"Jb", " ", "=", " ", 
              RowBox[{"JacobianBody", "[", 
               RowBox[{"Blist", ",", "\[Theta]i"}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Jt", " ", "=", " ", 
              RowBox[{"PseudoInverse", "[", "Jb", "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"\[Theta]i", "=", " ", 
              RowBox[{
               RowBox[{"Simplify", "[", 
                RowBox[{
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"{", "\[Theta]i", "}"}], "\[Transpose]"}], " ", 
                   "+", " ", 
                   RowBox[{"Jt", ".", "Vb"}]}], ")"}], "\[Transpose]"}], 
                "]"}], "[", 
               RowBox[{"[", "1", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
             RowBox[{"thetalist", " ", "=", " ", 
              RowBox[{"Insert", "[", 
               RowBox[{"thetalist", ",", "\[Theta]i", ",", 
                RowBox[{"i", "+", "2"}]}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"i", "++"}], ";", "\[IndentingNewLine]", 
             RowBox[{"Tsb", " ", "=", " ", 
              RowBox[{"FKinBody", "[", 
               RowBox[{"M", ",", "Blist", ",", "\[Theta]i"}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Vb", " ", "=", " ", 
              RowBox[{"Simplify", "[", 
               RowBox[{"MatrixLog6", "[", 
                RowBox[{
                 RowBox[{"Simplify", "[", 
                  RowBox[{"TransInv", "[", "Tsb", "]"}], "]"}], ".", "T"}], 
                "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", ",", 
            RowBox[{
             RowBox[{"success", " ", "=", " ", "False"}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Return", "[", 
              RowBox[{"{", 
               RowBox[{
                RowBox[{"thetalist", "[", 
                 RowBox[{"[", 
                  RowBox[{"i", "+", "1"}], "]"}], "]"}], ",", " ", 
                "success"}], "}"}], "]"}], ";"}]}], "]"}], ";"}]}], 
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"thetalist", "[", 
           RowBox[{"[", 
            RowBox[{"i", "+", "1"}], "]"}], "]"}], ",", " ", "success"}], 
         "}"}], "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675682824450754*^9, 
   3.6756828836706333`*^9}, {3.6756829858535852`*^9, 3.675683093476144*^9}, {
   3.675683163431617*^9, 3.675683301926485*^9}, {3.6756834458193197`*^9, 
   3.675683520674423*^9}, {3.675683569012762*^9, 3.675683584065415*^9}, 
   3.675683635500771*^9, {3.675684508273862*^9, 3.675684520687911*^9}, {
   3.675764977565001*^9, 3.675764979617189*^9}, {3.6757662451157312`*^9, 
   3.675766254498831*^9}, {3.675766317871736*^9, 3.675766345394107*^9}, {
   3.6757664078898573`*^9, 3.675766419041957*^9}, {3.6757665260531073`*^9, 
   3.6757665599652433`*^9}, {3.675766688881428*^9, 3.675766693414764*^9}, {
   3.675766760364307*^9, 3.675766898736827*^9}, {3.675766931380547*^9, 
   3.675766932915333*^9}, 3.675767022524777*^9, 3.675767053903921*^9, {
   3.675767094663486*^9, 3.675767157353602*^9}, {3.6757672038039017`*^9, 
   3.67576720815028*^9}, {3.675767263478516*^9, 3.675767302851701*^9}, {
   3.6757673469407463`*^9, 3.6757675158730087`*^9}, {3.675767609117689*^9, 
   3.6757676149377613`*^9}, 3.676110621848937*^9, {3.676110673192341*^9, 
   3.676110708132433*^9}, {3.676110758266004*^9, 3.676110779724792*^9}, {
   3.676110837502541*^9, 3.676110924302658*^9}, {3.676111015370205*^9, 
   3.676111023652515*^9}, {3.676111616884136*^9, 3.676111619101164*^9}, {
   3.67611165132108*^9, 3.67611182175666*^9}, 3.6761124388093357`*^9, {
   3.6761125310577183`*^9, 3.676112535417371*^9}, {3.67611278534865*^9, 
   3.676112852293083*^9}, {3.6761129469420633`*^9, 3.6761129566090517`*^9}, {
   3.6761133261675377`*^9, 3.6761133851728067`*^9}, {3.6761134570078707`*^9, 
   3.6761134886549597`*^9}, {3.676113519585043*^9, 3.676113581404831*^9}, {
   3.676113994051722*^9, 3.6761140495138607`*^9}, {3.67611471759483*^9, 
   3.676114727326268*^9}, {3.676114788083909*^9, 3.676114788267289*^9}, {
   3.67611493328951*^9, 3.6761149644016867`*^9}, {3.6761149993051443`*^9, 
   3.676115038096011*^9}, {3.676115419194528*^9, 3.676115484935451*^9}, {
   3.6761155900865583`*^9, 3.676115605985518*^9}, {3.676115760093829*^9, 
   3.676115778909306*^9}, {3.676115982499289*^9, 3.676116037133542*^9}, {
   3.6761161990700502`*^9, 3.676116233908482*^9}, {3.676116286507206*^9, 
   3.676116286712512*^9}, {3.67611639463448*^9, 3.676116464258135*^9}, {
   3.6761165873897543`*^9, 3.676116602580727*^9}, {3.6761174556753473`*^9, 
   3.67611745672952*^9}, {3.676117506535368*^9, 3.6761175229462357`*^9}, {
   3.676117600215383*^9, 3.6761176442364273`*^9}, {3.676117677849012*^9, 
   3.6761177082717953`*^9}, {3.67611812248459*^9, 3.6761181278825502`*^9}, {
   3.676118580166224*^9, 3.67611860485815*^9}, {3.676118651876894*^9, 
   3.6761187537386017`*^9}, {3.676118838160269*^9, 3.676118838374693*^9}, {
   3.67611887610327*^9, 3.676118940213615*^9}, {3.6761191096213007`*^9, 
   3.676119140757559*^9}, {3.6761192665343122`*^9, 3.676119273783136*^9}, {
   3.67611936142714*^9, 3.676119363325075*^9}, {3.676119488561606*^9, 
   3.676119492487294*^9}, {3.6761200703687267`*^9, 3.6761200707356052`*^9}, {
   3.6761202104386873`*^9, 3.676120268289008*^9}, {3.676120410677075*^9, 
   3.676120443293746*^9}, {3.676120474583932*^9, 3.676120475526285*^9}, {
   3.6761206125852633`*^9, 3.6761206205196333`*^9}, {3.6761234586125507`*^9, 
   3.676123489461384*^9}, {3.6761235483793917`*^9, 3.676123569634534*^9}, {
   3.676124493441423*^9, 3.676124518157651*^9}, {3.676124553443336*^9, 
   3.676124582966502*^9}, {3.6761246820730133`*^9, 3.676124716437358*^9}, {
   3.67612487549886*^9, 3.676124882235661*^9}, {3.676124956893395*^9, 
   3.676124958216114*^9}, {3.6761249904362507`*^9, 3.676124994364197*^9}, {
   3.676125048497746*^9, 3.676125189073619*^9}, {3.676125219923954*^9, 
   3.6761253767254953`*^9}, {3.6761261361020412`*^9, 3.676126180304117*^9}, {
   3.6761262318219957`*^9, 3.676126312208794*^9}, {3.6761263690337877`*^9, 
   3.6761264446350737`*^9}, {3.676126475578731*^9, 3.676126485467072*^9}, {
   3.676126538828438*^9, 3.676126562090301*^9}, {3.676126633785634*^9, 
   3.6761266365895157`*^9}, 3.6761268247458773`*^9, 3.676126912241247*^9, {
   3.676126960168599*^9, 3.676126960170166*^9}, {3.676127961375718*^9, 
   3.676127975299335*^9}, {3.676128268770321*^9, 3.676128269338847*^9}, 
   3.676128811729394*^9, {3.677836754519267*^9, 3.6778367717532873`*^9}, 
   3.67809625024065*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"IKinSpace", "[", 
      RowBox[{
      "Slist_", ",", "M_", ",", "T_", ",", "thetalist0_", ",", "eomg_", ",", 
       "ev_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Slist"}], ":", 
     RowBox[{
     "The", " ", "joint", " ", "screw", " ", "axes", " ", "in", " ", "the", 
      " ", "space", " ", "frame", " ", "when", " ", "the", " ", "manipulator",
       " ", "is", " ", "at", " ", "the", " ", "home", " ", "position"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"M", ":", 
     RowBox[{
      RowBox[{
      "The", " ", "home", " ", "configuration", " ", "of", " ", "the", " ", 
       "end"}], "-", "effector"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"T", ":", 
     RowBox[{
      RowBox[{"The", " ", "desired", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration", " ", "T"}]}]}], ",", "\n", 
    RowBox[{"thetalist0", ":", 
     RowBox[{
     "An", " ", "initial", " ", "guess", " ", "of", " ", "joint", " ", 
      "angles", " ", "that", " ", "are", " ", "close", " ", "to", " ", 
      "satisfying", " ", "T"}]}], ",", "\n", 
    RowBox[{"eomg", ":", 
     RowBox[{
      RowBox[{
      "A", " ", "small", " ", "positive", " ", "tolerance", " ", "on", " ", 
       "the", " ", "end"}], "-", 
      RowBox[{"effector", " ", "orientation", " ", 
       RowBox[{"error", ".", " ", "The"}], " ", "returned", " ", "joint", " ",
        "angles", " ", "must", " ", "give", " ", "an", " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "orientation", " ", "error", " ", "less", " ", "than", 
       " ", "eomg"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"ev", ":", 
     RowBox[{
      RowBox[{
      "A", " ", "small", " ", "positive", " ", "tolerance", " ", "on", " ", 
       "the", " ", "end"}], "-", 
      RowBox[{"effector", " ", "linear", " ", "position", " ", 
       RowBox[{"error", ".", " ", "The"}], " ", "returned", " ", "joint", " ",
        "angles", " ", "must", " ", "give", " ", "an", " ", "end"}], "-", 
      RowBox[{
      "effector", " ", "position", " ", "error", " ", "less", " ", "than", 
       " ", 
       RowBox[{
       "ev", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "The"}], " ", 
       "maximum", " ", "number", " ", "of", " ", "iterations", " ", "before", 
       " ", "the", " ", "algorithm", " ", "is", " ", "terminated", " ", "has",
        " ", "been", " ", "hardcoded", " ", "in", " ", "as", " ", "a", " ", 
       "variable", " ", "called", " ", 
       RowBox[{"maxiterations", ".", "It"}], " ", "is", " ", "set", " ", "to",
        " ", "20", " ", "at", " ", "the", " ", "start", " ", "of", " ", "the",
        " ", "function"}]}]}], ",", 
    RowBox[{"but", " ", "can", " ", "be", " ", "changed", " ", "if", " ", 
     RowBox[{
     "needed", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}],
      " ", 
     RowBox[{"thetalist", ":", 
      RowBox[{
      "Joint", " ", "angles", " ", "that", " ", "achieve", " ", "T", " ", 
       "within", " ", "the", " ", "specified", " ", "tolerances"}]}]}], ",", 
    RowBox[{
     RowBox[{"success", ":", 
      RowBox[{
       RowBox[{
       "A", " ", "logical", " ", "value", " ", "where", " ", "True", " ", 
        "means", " ", "that", " ", "the", " ", "function", " ", "found", " ", 
        "a", " ", "solution", " ", "and", " ", "False", "\n", "means", " ", 
        "that", " ", "it", " ", "ran", " ", "through", " ", "the", " ", "set",
         " ", "number", " ", "of", " ", "maximum", " ", "iterations", " ", 
        "without", " ", "finding", " ", "a", " ", "solution", "\n", "within", 
        " ", "the", " ", "tolerances", " ", "eomg", " ", "and", " ", 
        RowBox[{"ev", ".", "\[IndentingNewLine]", "Uses"}], " ", "an", " ", 
        "iterative", " ", "Newton"}], "-", 
       RowBox[{"Raphson", " ", "root"}], "-", 
       RowBox[{"finding", " ", "method"}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Exapmle", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Slist"}], " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", 
            RowBox[{"-", "0.550"}], ",", "0", ",", "0.045"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "1", ",", "0", ",", 
            RowBox[{"-", "0.850"}], ",", "0", ",", "0"}], "}"}], "}"}], 
         "\[Transpose]"}], ",", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], 
           "}"}], "}"}], "\[Transpose]"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"M", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "0.910"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"T", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "0.4"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "0.4"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"thetalist0", "=", 
      RowBox[{"{", 
       RowBox[{
       "0.1", ",", "0.1", ",", "0.1", ",", "0.1", ",", "0.1", ",", "0.1", ",",
         "0.1"}], "}"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"eomg", "=", "0.01"}], ";", "\n", 
     RowBox[{"ev", "=", "0.001"}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"thetalist", ",", " ", "success"}], "}"}], " ", "=", " ", 
      RowBox[{
       RowBox[{"IKinSpace", "[", 
        RowBox[{
        "Slist", ",", "M", ",", "T", ",", "thetalist0", ",", "eomg", ",", 
         "ev"}], "]"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Ouput", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"6.934838633950412", ",", "7.12127381090768", ",", 
            RowBox[{"-", "17.442412237293425"}], ",", 
            RowBox[{"-", "20.559005530185367"}], ",", 
            RowBox[{"-", "8.60119275865298"}], ",", "17.30667641254145", ",", 
            "13.698032964871741"}], "}"}], ",", "True"}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"IKinSpace", "[", 
     RowBox[{
     "Slist_", ",", "M_", ",", "T_", ",", "thetalist0_", ",", "eomg_", ",", 
      "ev_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"i", ",", 
        RowBox[{"maxiterations", " ", "=", " ", "20"}], ",", " ", "success", 
        ",", " ", "\[Theta]i", ",", "thetalist", ",", "Tsb", ",", "Vs", ",", 
        "Js", ",", "Jt"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"i", "=", "0"}], ";", "\[IndentingNewLine]", 
       RowBox[{"success", " ", "=", " ", "True"}], ";", "\[IndentingNewLine]", 
       RowBox[{"\[Theta]i", " ", "=", " ", "thetalist0"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"thetalist", " ", "=", " ", 
        RowBox[{"{", "\[Theta]i", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Tsb", " ", "=", " ", 
        RowBox[{"FKinSpace", "[", 
         RowBox[{"M", ",", "Slist", ",", "\[Theta]i"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Vs", " ", "=", " ", 
        RowBox[{
         RowBox[{"Adjoint", "[", "Tsb", "]"}], ".", 
         RowBox[{"MatrixLog6", "[", 
          RowBox[{
           RowBox[{"TransInv", "[", "Tsb", "]"}], ".", "T"}], "]"}]}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"While", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{"Norm", "[", 
            RowBox[{"Vs", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"1", ";;", "3"}], ",", "1"}], "]"}], "]"}], "]"}], ">",
            "eomg"}], " ", "||", " ", 
          RowBox[{
           RowBox[{"Norm", "[", 
            RowBox[{"Vs", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"4", ";;", "6"}], ",", "1"}], "]"}], "]"}], "]"}], ">",
            "ev"}]}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"If", "[", 
           RowBox[{
            RowBox[{"i", " ", "<", " ", "maxiterations"}], ",", 
            RowBox[{
             RowBox[{"Js", " ", "=", 
              RowBox[{"JacobianSpace", "[", 
               RowBox[{"Slist", ",", "\[Theta]i"}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Jt", " ", "=", " ", 
              RowBox[{"PseudoInverse", "[", "Js", "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"\[Theta]i", "=", " ", 
              RowBox[{
               RowBox[{"Simplify", "[", 
                RowBox[{
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"{", "\[Theta]i", "}"}], "\[Transpose]"}], " ", 
                   "+", " ", 
                   RowBox[{"Jt", ".", "Vs"}]}], ")"}], "\[Transpose]"}], 
                "]"}], "[", 
               RowBox[{"[", "1", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
             RowBox[{"thetalist", " ", "=", " ", 
              RowBox[{"Insert", "[", 
               RowBox[{"thetalist", ",", "\[Theta]i", ",", 
                RowBox[{"i", "+", "2"}]}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"i", "++"}], ";", "\[IndentingNewLine]", 
             RowBox[{"Tsb", " ", "=", " ", 
              RowBox[{"FKinSpace", "[", 
               RowBox[{"M", ",", "Slist", ",", "\[Theta]i"}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Vs", " ", "=", "  ", 
              RowBox[{
               RowBox[{"Adjoint", "[", "Tsb", "]"}], ".", 
               RowBox[{"MatrixLog6", "[", 
                RowBox[{
                 RowBox[{"TransInv", "[", "Tsb", "]"}], ".", "T"}], "]"}]}]}],
              ";"}], "\[IndentingNewLine]", ",", 
            RowBox[{
             RowBox[{"success", " ", "=", " ", "False"}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"Return", "[", 
              RowBox[{"{", 
               RowBox[{
                RowBox[{"thetalist", "[", 
                 RowBox[{"[", 
                  RowBox[{"i", "+", "1"}], "]"}], "]"}], ",", " ", 
                "success"}], "}"}], "]"}], ";"}]}], "]"}], ";"}]}], 
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"thetalist", "[", 
           RowBox[{"[", 
            RowBox[{"i", "+", "1"}], "]"}], "]"}], ",", " ", "success"}], 
         "}"}], "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675682824450754*^9, 
   3.6756828836706333`*^9}, {3.6756829858535852`*^9, 3.675683093476144*^9}, {
   3.675683163431617*^9, 3.675683301926485*^9}, {3.6756834458193197`*^9, 
   3.675683520674423*^9}, {3.675683569012762*^9, 3.675683584065415*^9}, 
   3.675683635500771*^9, {3.675684508273862*^9, 3.675684520687911*^9}, {
   3.675764977565001*^9, 3.675764979617189*^9}, {3.6757662451157312`*^9, 
   3.675766254498831*^9}, {3.675766317871736*^9, 3.675766345394107*^9}, {
   3.6757664078898573`*^9, 3.675766419041957*^9}, {3.6757665260531073`*^9, 
   3.6757665599652433`*^9}, {3.675766688881428*^9, 3.675766693414764*^9}, {
   3.675766760364307*^9, 3.675766898736827*^9}, {3.675766931380547*^9, 
   3.675766932915333*^9}, 3.675767022524777*^9, 3.675767053903921*^9, {
   3.675767094663486*^9, 3.675767157353602*^9}, {3.6757672038039017`*^9, 
   3.67576720815028*^9}, {3.675767263478516*^9, 3.675767302851701*^9}, {
   3.6757673469407463`*^9, 3.6757675158730087`*^9}, {3.675767609117689*^9, 
   3.6757676149377613`*^9}, 3.676110621848937*^9, {3.676110673192341*^9, 
   3.676110708132433*^9}, {3.676110758266004*^9, 3.676110779724792*^9}, {
   3.676110837502541*^9, 3.676110924302658*^9}, {3.676111015370205*^9, 
   3.676111023652515*^9}, {3.676111616884136*^9, 3.676111619101164*^9}, {
   3.67611165132108*^9, 3.67611182175666*^9}, 3.6761124388093357`*^9, {
   3.6761125310577183`*^9, 3.676112535417371*^9}, {3.67611278534865*^9, 
   3.676112852293083*^9}, {3.6761129469420633`*^9, 3.6761129566090517`*^9}, {
   3.6761133261675377`*^9, 3.6761133851728067`*^9}, {3.6761134570078707`*^9, 
   3.6761134886549597`*^9}, {3.676113519585043*^9, 3.676113581404831*^9}, {
   3.676113994051722*^9, 3.6761140495138607`*^9}, {3.67611471759483*^9, 
   3.676114727326268*^9}, {3.676114788083909*^9, 3.676114788267289*^9}, {
   3.67611493328951*^9, 3.6761149644016867`*^9}, {3.6761149993051443`*^9, 
   3.676115038096011*^9}, {3.676115419194528*^9, 3.676115484935451*^9}, {
   3.6761155900865583`*^9, 3.676115605985518*^9}, {3.676115760093829*^9, 
   3.676115778909306*^9}, {3.676115982499289*^9, 3.676116037133542*^9}, {
   3.6761161990700502`*^9, 3.676116233908482*^9}, {3.676116286507206*^9, 
   3.676116286712512*^9}, {3.67611639463448*^9, 3.676116464258135*^9}, {
   3.6761165873897543`*^9, 3.676116602580727*^9}, {3.6761174556753473`*^9, 
   3.67611745672952*^9}, {3.676117506535368*^9, 3.6761175229462357`*^9}, {
   3.676117600215383*^9, 3.6761176442364273`*^9}, {3.676117677849012*^9, 
   3.6761177082717953`*^9}, {3.67611812248459*^9, 3.6761181278825502`*^9}, {
   3.676118580166224*^9, 3.67611860485815*^9}, {3.676118651876894*^9, 
   3.6761187537386017`*^9}, {3.676118838160269*^9, 3.676118838374693*^9}, {
   3.67611887610327*^9, 3.676118940213615*^9}, {3.6761191096213007`*^9, 
   3.676119140757559*^9}, {3.6761192665343122`*^9, 3.676119273783136*^9}, {
   3.67611936142714*^9, 3.676119363325075*^9}, {3.676119488561606*^9, 
   3.676119492487294*^9}, {3.6761200703687267`*^9, 3.6761200707356052`*^9}, {
   3.6761202104386873`*^9, 3.676120268289008*^9}, {3.676120410677075*^9, 
   3.676120443293746*^9}, {3.676120474583932*^9, 3.676120475526285*^9}, {
   3.6761206125852633`*^9, 3.6761206205196333`*^9}, {3.6761234586125507`*^9, 
   3.676123489461384*^9}, {3.6761235483793917`*^9, 3.676123569634534*^9}, {
   3.676124493441423*^9, 3.676124518157651*^9}, {3.676124553443336*^9, 
   3.676124582966502*^9}, {3.6761246820730133`*^9, 3.676124716437358*^9}, {
   3.67612487549886*^9, 3.676124882235661*^9}, {3.676124956893395*^9, 
   3.676124958216114*^9}, {3.6761249904362507`*^9, 3.676124994364197*^9}, {
   3.676125048497746*^9, 3.676125189073619*^9}, {3.676125219923954*^9, 
   3.6761253767254953`*^9}, {3.6761261361020412`*^9, 3.676126180304117*^9}, {
   3.6761262318219957`*^9, 3.676126312208794*^9}, {3.6761263690337877`*^9, 
   3.6761264446350737`*^9}, {3.676126475578731*^9, 3.676126485467072*^9}, {
   3.676126538828438*^9, 3.676126562090301*^9}, {3.676126633785634*^9, 
   3.6761266365895157`*^9}, 3.6761269067489443`*^9, {3.676126941350791*^9, 
   3.676127003932433*^9}, {3.6761275996551237`*^9, 3.676127642283627*^9}, {
   3.6761279161285563`*^9, 3.6761279412801857`*^9}, {3.6761280105764437`*^9, 
   3.676128037774667*^9}, {3.676128092513379*^9, 3.676128127330976*^9}, 
   3.676128221290659*^9, {3.676128273312995*^9, 3.676128326326386*^9}, {
   3.676129382956367*^9, 3.676129408886795*^9}, {3.676129456274128*^9, 
   3.6761295004906263`*^9}, {3.676129547547868*^9, 3.676129590188859*^9}, 
   3.677237481686187*^9, {3.6772375151100492`*^9, 3.677237518364565*^9}, {
   3.677489743906653*^9, 3.677489745104307*^9}, {3.6774903277274837`*^9, 
   3.6774903278784943`*^9}, {3.677836773282992*^9, 3.677836780209435*^9}, 
   3.678096258635654*^9}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  8: DYNAMICS OF OPEN CHAINS",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}, {3.6756821024917507`*^9, 3.6756821283850946`*^9}, {
  3.675777792107044*^9, 3.67577781248841*^9}, {3.676210079313513*^9, 
  3.676210092833352*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{"ad", "[", "V_", "]"}], ":", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"Takes", " ", "6"}], "-", 
       RowBox[{
       "vector", " ", "spatial", " ", "velocity", " ", "and", " ", "returns", 
        " ", "the", " ", "corresponding", " ", "6", "x6", " ", 
        RowBox[{
         RowBox[{"matrix", "[", "adV", "]"}], ".", "#Used"}], " ", "to", " ", 
        "calculate", " ", "the", " ", "Lie", " ", 
        RowBox[{"bracket", "[", 
         RowBox[{"V1", ",", "V2"}], "]"}]}]}]}], "=", 
     RowBox[{
      RowBox[{"[", "adV1", "]"}], "V2"}]}], "\[IndentingNewLine]", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{
     "Example", ":", "\[IndentingNewLine]", " ", "Input", ":", 
      "\[IndentingNewLine]", "V"}], "  ", "=", " ", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"{", 
        RowBox[{"1", ",", "2", ",", "3", ",", "4", ",", "5", ",", "6"}], 
        "}"}], "}"}], "\[Transpose]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"ad", "[", "V", "]"}], "//", 
     RowBox[{"MatrixForm", "\[IndentingNewLine]", "\n", 
      RowBox[{"Output", ":", "\[IndentingNewLine]", 
       TagBox[
        RowBox[{"(", "\[NoBreak]", GridBox[{
           {"0", 
            RowBox[{"-", "3"}], "2", "0", "0", "0"},
           {"3", "0", 
            RowBox[{"-", "1"}], "0", "0", "0"},
           {
            RowBox[{"-", "2"}], "1", "0", "0", "0", "0"},
           {"0", 
            RowBox[{"-", "6"}], "5", "0", 
            RowBox[{"-", "3"}], "2"},
           {"6", "0", 
            RowBox[{"-", "4"}], "3", "0", 
            RowBox[{"-", "1"}]},
           {
            RowBox[{"-", "5"}], "4", "0", 
            RowBox[{"-", "2"}], "1", "0"}
          },
          
          GridBoxAlignment->{
           "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
            "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
          GridBoxSpacings->{"Columns" -> {
              Offset[0.27999999999999997`], {
               Offset[0.7]}, 
              Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
              Offset[0.2], {
               Offset[0.4]}, 
              Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
        Function[BoxForm`e$, 
         MatrixForm[BoxForm`e$]]]}]}]}]}], "\[IndentingNewLine]", "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"ad", "[", "V_", "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"adV", ",", " ", "w", ",", " ", "v"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"w", "=", 
        RowBox[{"VecToso3", "[", 
         RowBox[{"V", "[", 
          RowBox[{"[", 
           RowBox[{"1", ";;", "3"}], "]"}], "]"}], "]"}]}], ";", "\n", 
       RowBox[{"v", "=", 
        RowBox[{"VecToso3", "[", 
         RowBox[{"V", "[", 
          RowBox[{"[", 
           RowBox[{"4", ";;", "6"}], "]"}], "]"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"adV", " ", "=", " ", 
        RowBox[{"ArrayFlatten", "[", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{
             RowBox[{"w", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", 
                RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], ",", "0"}], "}"}], 
           ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"v", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", 
                RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], ",", 
             RowBox[{"w", "[", 
              RowBox[{"[", 
               RowBox[{
                RowBox[{"1", ";;", "3"}], ",", 
                RowBox[{"1", ";;", "3"}]}], "]"}], "]"}]}], "}"}]}], "}"}], 
         "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "adV", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.6757780749103928`*^9, 3.675778092666944*^9}, {
   3.6757781317770443`*^9, 3.675778148103283*^9}, {3.675778193206167*^9, 
   3.675778200868908*^9}, {3.6757782344147387`*^9, 3.675778299338944*^9}, {
   3.676118088954138*^9, 3.676118088960745*^9}, {3.676210121948041*^9, 
   3.6762101271997538`*^9}, {3.676212687043581*^9, 3.676212739302689*^9}, {
   3.676212821098608*^9, 3.6762128555983953`*^9}, {3.676212894839349*^9, 
   3.676212899651846*^9}, {3.676213280902792*^9, 3.6762133170611897`*^9}, {
   3.6762133722711687`*^9, 3.676213420278404*^9}, {3.676213490348229*^9, 
   3.676213507276989*^9}, {3.6762136472488127`*^9, 3.676213647419341*^9}, {
   3.676213686097926*^9, 3.676213707298381*^9}, {3.6762137454501057`*^9, 
   3.676213746612797*^9}, {3.6762142650551157`*^9, 3.676214334303116*^9}, {
   3.676216025217536*^9, 3.676216073648109*^9}, {3.676216233956223*^9, 
   3.676216234458477*^9}, {3.6762944072751217`*^9, 3.676294572531825*^9}, 
   3.677238397377141*^9, 3.6772388506211557`*^9, {3.677238962412993*^9, 
   3.677238962996073*^9}, {3.677245446690844*^9, 3.6772454628094*^9}, {
   3.678096299605474*^9, 3.678096309489983*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"InverseDynamics", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "ddthetalist_", ",", "g_", ",", 
       "Ftip_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "variables"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "rates"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"ddthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "accelerations"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"Gravity", " ", "vector", " ", "g"}]}], ",", 
    RowBox[{"Ftip", ":", 
     RowBox[{
      RowBox[{
      "Spatial", " ", "force", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", "expressed", " ", "in", " ", "frame", " ", 
       RowBox[{"{", 
        RowBox[{"n", "+", "1"}], "}"}]}]}]}], ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{"List", " ", "of", " ", "link", " ", "frames", " ", 
      RowBox[{"{", "i", "}"}], " ", "relative", " ", "to", " ", 
      RowBox[{"{", 
       RowBox[{"i", "-", "1"}], "}"}], " ", "at", " ", "the", " ", "home", 
      " ", "position"}]}], ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{"Slist", ":", 
       RowBox[{
       "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
        " ", "in", " ", "a", " ", "space", " ", 
        RowBox[{
        "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
         "Returns"}], " ", "taulist"}], ":", 
       RowBox[{
        RowBox[{"The", " ", "n"}], "-", 
        RowBox[{"vector", " ", "of", " ", "required", " ", "joint", " ", 
         RowBox[{"forces", "/", 
          RowBox[{"torques", ".", "\[IndentingNewLine]", "This"}]}], " ", 
         "function", " ", "uses", " ", "forward"}], "-", 
        RowBox[{"backward", " ", "Newton"}], "-", 
        RowBox[{
        "Euler", " ", "iterations", " ", "to", " ", "solve", " ", "the", " ", 
         
         RowBox[{"equation", ":", "\[IndentingNewLine]", "taulist"}]}]}]}], 
      "=", 
      RowBox[{
       RowBox[{"Mlist", 
        RowBox[{"(", "thetalist", ")"}], "ddthetalist"}], "+", 
       RowBox[{"c", 
        RowBox[{"(", 
         RowBox[{"thetalist", ",", "dthetalist"}], ")"}]}], "+", 
       RowBox[{"g", 
        RowBox[{"(", "thetalist", ")"}]}], "+", 
       RowBox[{"Jtr", 
        RowBox[{"(", "thetalist", ")"}], "Ftip"}]}]}], "\[IndentingNewLine]", 
     ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", 
     RowBox[{"ddthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"2", ",", "1.5", ",", "1"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Ftip", "=", 
      RowBox[{"{", 
       RowBox[{"1", ",", "1", ",", "1", ",", "1", ",", "1", ",", "1"}], 
       "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"taulist", " ", "=", " ", 
      RowBox[{
       RowBox[{"InverseDynamics", "[", 
        RowBox[{
        "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",", 
         "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}], 
       "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{"12.28740936512365", ",", 
          RowBox[{"-", "20.555491300883507"}], ",", 
          RowBox[{"-", "2.9403622930411073"}]}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"InverseDynamics", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "ddthetalist_", ",", "g_", ",", 
      "Ftip_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "taulist", ",", " ", "n", " ", ",", " ", "Mi", ",", "Ai", ",", "Ti", 
        ",", "Vi", ",", "Vdi", ",", " ", "Fi"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{"(*", 
       RowBox[{
        RowBox[{"**", "**"}], "*", 
        RowBox[{"INITIALISATION", "**"}]}], "******)"}], 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", "=", 
        RowBox[{"Length", "[", "Mlist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Mi", " ", "=", " ", 
        RowBox[{"{", 
         RowBox[{"Mlist", "[", 
          RowBox[{"[", "1", "]"}], "]"}], "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Ai", "=", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Adjoint", "[", 
           RowBox[{"TransInv", "[", 
            RowBox[{"Mi", "[", 
             RowBox[{"[", "1", "]"}], "]"}], "]"}], "]"}], ".", 
          RowBox[{"Slist", "[", 
           RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Ti", "=", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Mlist", "[", 
           RowBox[{"[", "1", "]"}], "]"}], ".", 
          RowBox[{"MatrixExp6", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{
              RowBox[{"Ai", "[", 
               RowBox[{"[", "1", "]"}], "]"}], "*", 
              RowBox[{"thetalist", "[", 
               RowBox[{"[", "1", "]"}], "]"}]}], "}"}], "\[Transpose]"}], 
           "]"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Vi", "=", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"AppendTo", "[", 
        RowBox[{"Vi", ",", 
         RowBox[{
          RowBox[{
           RowBox[{"Adjoint", "[", 
            RowBox[{"TransInv", "[", 
             RowBox[{"Ti", "[", 
              RowBox[{"[", "1", "]"}], "]"}], "]"}], "]"}], ".", 
           RowBox[{"{", 
            RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
            "}"}]}], "+", 
          RowBox[{
           RowBox[{"Ai", "[", 
            RowBox[{"[", "1", "]"}], "]"}], "*", 
           RowBox[{"dthetalist", "[", 
            RowBox[{"[", "1", "]"}], "]"}]}]}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Vdi", "=", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"AppendTo", "[", 
        RowBox[{"Vdi", ",", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"Adjoint", "[", 
              RowBox[{"TransInv", "[", 
               RowBox[{"Ti", "[", 
                RowBox[{"[", "1", "]"}], "]"}], "]"}], "]"}], ".", 
             RowBox[{"{", 
              RowBox[{"0", ",", "0", ",", "0", ",", 
               RowBox[{"-", 
                RowBox[{"g", "[", 
                 RowBox[{"[", "1", "]"}], "]"}]}], ",", 
               RowBox[{"-", 
                RowBox[{"g", "[", 
                 RowBox[{"[", "2", "]"}], "]"}]}], ",", 
               RowBox[{"-", 
                RowBox[{"g", "[", 
                 RowBox[{"[", "3", "]"}], "]"}]}]}], "}"}]}], ")"}], "+", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"(", 
              RowBox[{
               RowBox[{"ad", "[", 
                RowBox[{
                 RowBox[{"{", 
                  RowBox[{"Vi", "[", 
                   RowBox[{"[", "1", "]"}], "]"}], "}"}], "\[Transpose]"}], 
                "]"}], ".", 
               RowBox[{"Ai", "[", 
                RowBox[{"[", "1", "]"}], "]"}]}], ")"}], "*", 
             RowBox[{"dthetalist", "[", 
              RowBox[{"[", "1", "]"}], "]"}]}], ")"}], "+", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"Ai", "[", 
              RowBox[{"[", "1", "]"}], "]"}], "*", 
             RowBox[{"ddthetalist", "[", 
              RowBox[{"[", "1", "]"}], "]"}]}], ")"}]}], ")"}]}], "]"}], ";", 
       "\n", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**"}], "*", 
         RowBox[{"**", "**", "**", "**", "**", "**", "**", "**"}]}], 
        "******)"}], "\n", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**", "Forward"}], " ", "Iteration"}], "******)"}], 
       "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{"AppendTo", "[", 
            RowBox[{"Mi", ",", 
             RowBox[{
              RowBox[{"Mi", "[", 
               RowBox[{"[", 
                RowBox[{"-", "1"}], "]"}], "]"}], ".", 
              RowBox[{"Mlist", "[", 
               RowBox[{"[", "i", "]"}], "]"}]}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"Ai", ",", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{"Adjoint", "[", 
                RowBox[{"TransInv", "[", 
                 RowBox[{"Mi", "[", 
                  RowBox[{"[", "i", "]"}], "]"}], "]"}], "]"}], ".", 
               RowBox[{"Slist", "[", 
                RowBox[{"[", "i", "]"}], "]"}]}], "}"}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"Ti", ",", 
             RowBox[{
              RowBox[{"Mlist", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ".", 
              RowBox[{"MatrixExp6", "[", 
               RowBox[{
                RowBox[{"{", 
                 RowBox[{
                  RowBox[{"Flatten", "[", 
                   RowBox[{"Ai", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], "]"}], "*", 
                  RowBox[{"thetalist", "[", 
                   RowBox[{"[", "i", "]"}], "]"}]}], "}"}], "\[Transpose]"}], 
               "]"}]}]}], "]"}], ";", "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"Vi", ",", 
             RowBox[{
              RowBox[{
               RowBox[{"Adjoint", "[", 
                RowBox[{"TransInv", "[", 
                 RowBox[{"Ti", "[", 
                  RowBox[{"[", "i", "]"}], "]"}], "]"}], "]"}], ".", 
               RowBox[{"Flatten", "[", 
                RowBox[{"Vi", "[", 
                 RowBox[{"[", 
                  RowBox[{"i", "-", "1"}], "]"}], "]"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"Flatten", "[", 
                RowBox[{"Ai", "[", 
                 RowBox[{"[", "i", "]"}], "]"}], "]"}], "*", 
               RowBox[{"dthetalist", "[", 
                RowBox[{"[", "i", "]"}], "]"}]}]}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"Vdi", ",", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"Adjoint", "[", 
                  RowBox[{"TransInv", "[", 
                   RowBox[{"Ti", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], "]"}], "]"}], ".", 
                 RowBox[{"Flatten", "[", 
                  RowBox[{"Vdi", "[", 
                   RowBox[{"[", 
                    RowBox[{"i", "-", "1"}], "]"}], "]"}], "]"}]}], ")"}], 
               "+", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"ad", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"Flatten", "[", 
                    RowBox[{"Vi", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], "]"}], "}"}], 
                    "\[Transpose]"}], "]"}], ".", 
                   RowBox[{"Flatten", "[", 
                    RowBox[{"Ai", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ")"}], "*", 
                 RowBox[{"dthetalist", "[", 
                  RowBox[{"[", "i", "]"}], "]"}]}], ")"}], "+", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"Flatten", "[", 
                  RowBox[{"Ai", "[", 
                   RowBox[{"[", "i", "]"}], "]"}], "]"}], "*", 
                 RowBox[{"ddthetalist", "[", 
                  RowBox[{"[", "i", "]"}], "]"}]}], ")"}]}], ")"}]}], "]"}], 
           ";"}], "\[IndentingNewLine]", "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "2", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**"}], "*", 
         RowBox[{"**", "**", "**", "**", "**", "**", "**", "**"}]}], 
        "******)"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**"}], "*", 
         RowBox[{"INITIALISATION", "**"}]}], "******)"}], 
       "\[IndentingNewLine]", 
       RowBox[{"Fi", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"n", ",", "1"}], "}"}]}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"Fi", "[", 
         RowBox[{"[", "n", "]"}], "]"}], "=", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"Adjoint", "[", 
             RowBox[{"TransInv", "[", 
              RowBox[{"Ti", "[", 
               RowBox[{"[", "n", "]"}], "]"}], "]"}], "]"}], "\[Transpose]"}],
            ".", "Ftip"}], ")"}], "+", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"Glist", "[", 
            RowBox[{"[", "n", "]"}], "]"}], ".", 
           RowBox[{"Flatten", "[", 
            RowBox[{"Vdi", "[", 
             RowBox[{"[", "n", "]"}], "]"}], "]"}]}], ")"}], "-", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"ad", "[", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"Flatten", "[", 
                RowBox[{"Vi", "[", 
                 RowBox[{"[", "n", "]"}], "]"}], "]"}], "}"}], 
              "\[Transpose]"}], "]"}], "\[Transpose]"}], ".", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"Flatten", "[", 
              RowBox[{"Vi", "[", 
               RowBox[{"[", "n", "]"}], "]"}], "]"}], ".", 
             RowBox[{"Glist", "[", 
              RowBox[{"[", "n", "]"}], "]"}]}], ")"}]}], ")"}]}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"taulist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"n", ",", "1"}], "}"}]}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"taulist", "[", 
         RowBox[{"[", "n", "]"}], "]"}], "=", 
        RowBox[{
         RowBox[{"Fi", "[", 
          RowBox[{"[", "n", "]"}], "]"}], ".", 
         RowBox[{"Flatten", "[", 
          RowBox[{"Ai", "[", 
           RowBox[{"[", "n", "]"}], "]"}], "]"}]}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**"}], "*", 
         RowBox[{"**", "**", "**", "**", "**", "**", "**", "**"}]}], 
        "******)"}], "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**", "Backward"}], " ", "Iteration"}], "*****)"}], 
       "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{
            RowBox[{"Fi", "[", 
             RowBox[{"[", "i", "]"}], "]"}], "=", 
            RowBox[{
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"Adjoint", "[", 
                 RowBox[{"TransInv", "[", 
                  RowBox[{"Ti", "[", 
                   RowBox[{"[", "i", "]"}], "]"}], "]"}], "]"}], 
                "\[Transpose]"}], ".", "Ftip"}], ")"}], "+", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"Glist", "[", 
                RowBox[{"[", "i", "]"}], "]"}], ".", 
               RowBox[{"Flatten", "[", 
                RowBox[{"Vdi", "[", 
                 RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ")"}], "-", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"ad", "[", 
                 RowBox[{
                  RowBox[{"{", 
                   RowBox[{"Flatten", "[", 
                    RowBox[{"Vi", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], "]"}], "}"}], 
                  "\[Transpose]"}], "]"}], "\[Transpose]"}], ".", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"Flatten", "[", 
                  RowBox[{"Vi", "[", 
                   RowBox[{"[", "i", "]"}], "]"}], "]"}], ".", 
                 RowBox[{"Glist", "[", 
                  RowBox[{"[", "i", "]"}], "]"}]}], ")"}]}], ")"}]}]}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{
            RowBox[{"taulist", "[", 
             RowBox[{"[", "i", "]"}], "]"}], "=", 
            RowBox[{
             RowBox[{"Fi", "[", 
              RowBox[{"[", "i", "]"}], "]"}], ".", 
             RowBox[{"Flatten", "[", 
              RowBox[{"Ai", "[", 
               RowBox[{"[", "i", "]"}], "]"}], "]"}]}]}], ";"}], 
          "\[IndentingNewLine]", "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", 
           RowBox[{"n", "-", "1"}], ",", "1", ",", 
           RowBox[{"-", "1"}]}], "}"}]}], "]"}], ";", "\n", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"**", "**"}], "*", 
         RowBox[{"**", "**", "**", "**", "**", "**", "**", "**"}]}], 
        "******)"}], "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "taulist", "]"}], ";"}]}], 
     "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.6772391025485983`*^9, 3.6772391030813503`*^9}, 
   3.677244355984013*^9, {3.677244432148254*^9, 3.677244504323576*^9}, {
   3.677244554123225*^9, 3.677244576679357*^9}, {3.677244685399939*^9, 
   3.677244756023776*^9}, 3.67724480098313*^9, {3.677245676616014*^9, 
   3.67724568111721*^9}, {3.677246001533386*^9, 3.6772461750994062`*^9}, {
   3.677246209080035*^9, 3.677246229640416*^9}, {3.677246271327312*^9, 
   3.677246312154667*^9}, {3.677246368623028*^9, 3.677246384540496*^9}, {
   3.677246494811397*^9, 3.677246548249215*^9}, {3.677246627246484*^9, 
   3.677246772095085*^9}, {3.67724729367937*^9, 3.677247340339321*^9}, {
   3.677247372979681*^9, 3.677247391909115*^9}, {3.677247561296047*^9, 
   3.6772475644357*^9}, {3.6772476504484663`*^9, 3.6772476565869627`*^9}, {
   3.6772476975735493`*^9, 3.677247723351466*^9}, {3.6772477583612003`*^9, 
   3.677247782172574*^9}, {3.677247813053689*^9, 3.677247821497436*^9}, {
   3.67724786098481*^9, 3.677247887660157*^9}, {3.677247918383231*^9, 
   3.6772479227268*^9}, {3.677248051666149*^9, 3.677248152419401*^9}, {
   3.6772481871568413`*^9, 3.677248213947154*^9}, {3.677248311113554*^9, 
   3.677248386749319*^9}, {3.677248426650477*^9, 3.6772484275017853`*^9}, {
   3.6772485383032217`*^9, 3.677248588205284*^9}, {3.677248681364223*^9, 
   3.6772486890120153`*^9}, {3.6772487541715918`*^9, 
   3.6772488872632723`*^9}, {3.677248932718923*^9, 3.67724893344909*^9}, {
   3.677249002513879*^9, 3.677249115230443*^9}, {3.6772491452713337`*^9, 
   3.677249198829335*^9}, 3.677249242707559*^9, {3.677249353801366*^9, 
   3.6772494274284277`*^9}, {3.6772495211811447`*^9, 3.677249528268223*^9}, {
   3.677249676107225*^9, 3.677249714971898*^9}, {3.677249813626606*^9, 
   3.6772501354896927`*^9}, 3.677250272338114*^9, {3.677250388806677*^9, 
   3.677250512226314*^9}, {3.6772505488546057`*^9, 3.677250548946186*^9}, {
   3.6772506625894833`*^9, 3.677250834033412*^9}, {3.6772508642856293`*^9, 
   3.677250926794492*^9}, {3.677251890854176*^9, 3.677251908541988*^9}, {
   3.677251955063167*^9, 3.677251982543026*^9}, {3.677252082789035*^9, 
   3.677252135707983*^9}, {3.6772547120542088`*^9, 3.677254778367365*^9}, {
   3.677254809940134*^9, 3.677254810694992*^9}, {3.677254936380105*^9, 
   3.6772550358368607`*^9}, {3.677255175840102*^9, 3.677255202520162*^9}, {
   3.6772552720212193`*^9, 3.677255272480659*^9}, 3.677255321516757*^9, 
   3.6772554026994257`*^9, {3.677255533302904*^9, 3.677255548144637*^9}, {
   3.677255924206114*^9, 3.6772559564821453`*^9}, {3.677255990775902*^9, 
   3.6772560084051123`*^9}, {3.677256048499477*^9, 3.677256101134179*^9}, {
   3.677256255849882*^9, 3.677256431791498*^9}, {3.677256470560192*^9, 
   3.677256537679666*^9}, {3.6772567050765944`*^9, 3.6772567532846327`*^9}, {
   3.677256819553606*^9, 3.677256943552742*^9}, {3.677257016408537*^9, 
   3.677257041328187*^9}, {3.677257086092701*^9, 3.677257325334237*^9}, {
   3.6772573735555563`*^9, 3.677257448101842*^9}, {3.6772575152209272`*^9, 
   3.677257535140091*^9}, {3.6772576590287313`*^9, 3.677257677722865*^9}, {
   3.677257729412159*^9, 3.677257788180209*^9}, {3.677257833538631*^9, 
   3.677257942254745*^9}, {3.6772585288153133`*^9, 3.677258592724414*^9}, {
   3.6772588085902243`*^9, 3.677258819342699*^9}, {3.677258983718956*^9, 
   3.6772590167734127`*^9}, {3.677259130975376*^9, 3.6772591650591803`*^9}, {
   3.677259274513762*^9, 3.677259372680434*^9}, {3.677259417310178*^9, 
   3.6772596693390636`*^9}, {3.677259912324069*^9, 3.6772600207718277`*^9}, {
   3.677260060802122*^9, 3.677260207489208*^9}, {3.677260242637308*^9, 
   3.677260382454686*^9}, {3.67726042036277*^9, 3.677260507331476*^9}, {
   3.67726056708534*^9, 3.677260571117894*^9}, 3.677260611982997*^9, {
   3.677260659686365*^9, 3.677260696376569*^9}, {3.677260881888175*^9, 
   3.677260909792899*^9}, {3.677260943377907*^9, 3.677260943956663*^9}, {
   3.677261046331066*^9, 3.67726106321067*^9}, {3.6772611189125423`*^9, 
   3.677261159467651*^9}, {3.6772612579889803`*^9, 3.6772613036200323`*^9}, {
   3.677261343258348*^9, 3.67726164112537*^9}, {3.67726168069374*^9, 
   3.677261684921814*^9}, {3.677261739688339*^9, 3.677261795521442*^9}, {
   3.67726184472169*^9, 3.677261850163794*^9}, {3.677261889920115*^9, 
   3.677261917748721*^9}, {3.677261953319784*^9, 3.677262018774967*^9}, {
   3.6772620665573263`*^9, 3.67726206684867*^9}, {3.677262109500868*^9, 
   3.67726226901488*^9}, {3.677262344350646*^9, 3.6772623894785347`*^9}, {
   3.677262443299412*^9, 3.677262547832739*^9}, {3.6772626032030478`*^9, 
   3.677262634813252*^9}, 3.6772627141364517`*^9, {3.6772627478397913`*^9, 
   3.677262922790249*^9}, {3.6772630014493*^9, 3.6772630257156763`*^9}, 
   3.677315262280599*^9, {3.67749043474179*^9, 3.677490435196591*^9}, 
   3.678096323305634*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"MassMatrix", "[", 
      RowBox[{"thetalist_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], 
      "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "variables"}]}], 
    ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
      " ", "in", " ", "a", " ", "space", " ", 
      RowBox[{
      "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}],
       " ", "M"}], ":", 
     RowBox[{
      RowBox[{
      "The", " ", "numerical", " ", "inertia", " ", "matrix", " ", "M", 
       RowBox[{"(", "thetalist", ")"}], " ", "of", " ", "an", " ", "n"}], "-", 
      RowBox[{
      "joint", " ", "serial", " ", "chain", " ", "at", " ", "the", "\n", 
       "given", " ", "configuration", " ", 
       RowBox[{"thetalist", ".", "\[IndentingNewLine]", "This"}], " ", 
       "function", " ", "calls", " ", "InverseDynamics", " ", "n", " ", 
       "times"}]}]}], ",", 
    RowBox[{
    "each", " ", "time", " ", "passing", " ", "a", " ", "ddthetalist", " ", 
     "vector", "\n", "with", " ", "a", " ", "single", " ", "element", " ", 
     "equal", " ", "to", " ", "one", " ", "and", " ", "all", " ", "other", 
     " ", "inputs", " ", "set", " ", "to", " ", 
     RowBox[{"zero", ".", "\[IndentingNewLine]", "Each"}], " ", "call", " ", 
     "of", " ", "InverseDynamics", " ", "generates", " ", "a", " ", "single", 
     " ", "column"}], ",", 
    RowBox[{
     RowBox[{
     "and", " ", "these", " ", "columns", " ", "are", " ", "assembled", " ", 
      "to", " ", "create", " ", "the", " ", "inertia", " ", 
      RowBox[{"matrix", "."}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"M", " ", "=", " ", 
      RowBox[{
       RowBox[{"MassMatrix", "[", 
        RowBox[{"thetalist", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
        "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"3.0865629109397004", 
              RowBox[{"-", "0.28598437110125424"}], 
              RowBox[{"-", "0.007184263909440675"}]},
             {"0.", "0.8849014121834332", "0.4321573682931936"},
             {"0.", "0.", "0.19163085751427505"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"MassMatrix", "[", 
     RowBox[{"thetalist_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], 
     "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "n", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",", "Ftip", ",",
         "taulist", ",", "M"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"dthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"g", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "3"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Ftip", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "6"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"M", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"ddthetalist", " ", "=", " ", 
           RowBox[{"ConstantArray", "[", 
            RowBox[{"0", ",", "n"}], "]"}]}], ";", 
          RowBox[{
           RowBox[{"ddthetalist", "[", 
            RowBox[{"[", "i", "]"}], "]"}], " ", "=", " ", "1"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"taulist", " ", "=", " ", 
           RowBox[{"InverseDynamics", "[", 
            RowBox[{
            "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",",
              "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}]}], 
          ";", "\[IndentingNewLine]", 
          RowBox[{"AppendTo", "[", 
           RowBox[{"M", ",", "taulist"}], "]"}], ";"}], "\[IndentingNewLine]",
          ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "M", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.677315383100857*^9, 3.6773153874097767`*^9}, {
   3.677315451577257*^9, 3.67731550527804*^9}, {3.677315540973567*^9, 
   3.67731554925203*^9}, {3.677315687843329*^9, 3.677315714044332*^9}, {
   3.677316340908476*^9, 3.6773165375560226`*^9}, {3.677316574393567*^9, 
   3.6773165751686068`*^9}, {3.677316741164989*^9, 3.677316779517893*^9}, {
   3.67731693109394*^9, 3.677316983329446*^9}, {3.677317041029996*^9, 
   3.677317088346855*^9}, {3.6773171561703167`*^9, 3.677317188692944*^9}, {
   3.677318007172571*^9, 3.677318025098009*^9}, {3.677490445750225*^9, 
   3.677490446389659*^9}, 3.678096332476907*^9, {3.681451008340869*^9, 
   3.681451020942246*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"VelQuadraticForces", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "Mlist_", ",", "Glist_", ",", 
       "Slist_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "variables"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "rates"}]}], ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{
     RowBox[{"Slist", ":", 
      RowBox[{
      "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
       " ", "in", " ", "a", " ", "space", " ", 
       RowBox[{
       "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        "Returns"}], " ", "c"}], ":", 
      RowBox[{"The", " ", "vector", " ", "c", 
       RowBox[{"(", 
        RowBox[{"thetalist", ",", "dthetalist"}], ")"}], " ", "of", " ", 
       "Coriolis", " ", "and", " ", "centripetal", " ", "terms", "\n", "for", 
       " ", "a", " ", "given", " ", "thetalist", " ", "and", " ", 
       RowBox[{"dthetalist", ".", "\n", "This"}], " ", "function", " ", 
       "calls", " ", "InverseDynamics", " ", "with", " ", "g"}]}], "=", "0"}],
     ",", 
    RowBox[{"Ftip", "=", "0"}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{"and", " ", "ddthetalist"}], "=", "0."}], "\[IndentingNewLine]",
      ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", "\n", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"c", " ", "=", " ", 
      RowBox[{
       RowBox[{"VelQuadraticForces", "[", 
        RowBox[{
        "thetalist", ",", "dthetalist", ",", "Mlist", ",", "Glist", ",", 
         "Slist"}], "]"}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", "\[IndentingNewLine]", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", "0.027703940237666886"}], ",", 
           RowBox[{"-", "0.006891320068248913"}]}], "}"}]}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"VelQuadraticForces", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "Mlist_", ",", "Glist_", ",", 
      "Slist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"c", ",", "ddthetalist", ",", "g", ",", "Ftip", ",", "n"}], 
       "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"ddthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"g", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "3"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Ftip", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "6"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"c", " ", "=", " ", 
        RowBox[{"InverseDynamics", "[", 
         RowBox[{
         "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",", 
          "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "c", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.677317411138452*^9, 3.677317488094689*^9}, {
   3.677317526836624*^9, 3.6773175326578627`*^9}, {3.677317664909581*^9, 
   3.6773177047834806`*^9}, {3.677317735802031*^9, 3.677317802344627*^9}, {
   3.677317840881192*^9, 3.677317897783889*^9}, {3.677317993699333*^9, 
   3.677317999721787*^9}, {3.677318816262457*^9, 3.6773188165402718`*^9}, {
   3.677490452561892*^9, 3.6774904527054863`*^9}, 3.6780963385542927`*^9, {
   3.678449190708435*^9, 3.6784492009951878`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"GravityForces", "[", 
      RowBox[{
      "thetalist_", ",", "g_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], 
      "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "variables"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"3", "-", 
      RowBox[{
      "vector", " ", "for", " ", "gravitational", " ", "acceleration"}]}]}], 
    ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{
     RowBox[{"Slist", ":", 
      RowBox[{
      "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
       " ", "in", " ", "a", " ", "space", " ", 
       RowBox[{
       "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        "Returns"}], " ", "grav"}], ":", 
      RowBox[{"The", " ", "joint", " ", 
       RowBox[{"forces", "/", "torques"}], " ", "required", " ", "to", " ", 
       "overcome", " ", "gravity", " ", "at", " ", "thetalist", "\n", "This", 
       " ", "function", " ", "calls", " ", "InverseDynamics", " ", "with", 
       " ", "Ftip"}]}], "=", "0"}], ",", 
    RowBox[{"dthetalist", "=", "0"}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{"and", " ", "ddthetalist"}], "=", "0."}], "\[IndentingNewLine]",
      ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"grav", " ", "=", " ", 
      RowBox[{
       RowBox[{"GravityForces", "[", 
        RowBox[{
        "thetalist", ",", "g", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
        "]"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{"3.291711438237281", ",", 
          RowBox[{"-", "22.813661135010662"}], ",", 
          RowBox[{"-", "5.4415891999683605"}]}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"GravityForces", "[", 
     RowBox[{
     "thetalist_", ",", "g_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], 
     "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "grav", ",", "n", ",", "dthetalist", ",", "ddthetalist", ",", "Ftip"}],
        "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"dthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"ddthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Ftip", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "6"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"grav", " ", "=", " ", 
        RowBox[{"InverseDynamics", "[", 
         RowBox[{
         "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",", 
          "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "grav", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.6773181226916533`*^9, 3.677318187212339*^9}, {
   3.677318223833849*^9, 3.677318304494225*^9}, {3.677318387435951*^9, 
   3.677318391354529*^9}, {3.677318421693688*^9, 3.67731848053815*^9}, {
   3.677318518292635*^9, 3.677318589025486*^9}, {3.677318623307369*^9, 
   3.677318659212624*^9}, {3.6773187956764297`*^9, 3.677318796630522*^9}, {
   3.677318844715273*^9, 3.6773188452284517`*^9}, {3.6774899146551743`*^9, 
   3.6774899486273317`*^9}, 3.6780963445239964`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"EndEffectorForces", "[", 
      RowBox[{
      "thetalist_", ",", "Ftip_", ",", "Mlist_", ",", "Glist_", ",", 
       "Slist_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "variables"}]}], 
    ",", 
    RowBox[{"Ftip", ":", 
     RowBox[{
      RowBox[{
      "Spatial", " ", "force", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", "expressed", " ", "in", " ", "frame", " ", 
       RowBox[{"{", 
        RowBox[{"n", "+", "1"}], "}"}]}]}]}], ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{
     RowBox[{"Slist", ":", 
      RowBox[{
      "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
       " ", "in", " ", "a", " ", "space", " ", 
       RowBox[{
       "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        "Returns"}], " ", "JTFtip"}], ":", 
      RowBox[{
       RowBox[{
       "The", " ", "joint", " ", "forces", " ", "and", " ", "torques", " ", 
        "required", " ", "only", " ", "to", " ", "create", " ", "the", " ", 
        "end"}], "-", 
       RowBox[{"effector", " ", "force", " ", 
        RowBox[{"Ftip", ".", "\[IndentingNewLine]", "This"}], " ", "function",
         " ", "calls", " ", "InverseDynamics", " ", "with", " ", "g"}]}]}], 
     "=", "0"}], ",", 
    RowBox[{"dthetalist", "=", "0"}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{"and", " ", "ddthetalist"}], "=", "0."}], "\[IndentingNewLine]",
      ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Ftip", "=", 
      RowBox[{"{", 
       RowBox[{"1", ",", "1", ",", "1", ",", "1", ",", "1", ",", "1"}], 
       "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"JTFtip", " ", "=", " ", 
      RowBox[{
       RowBox[{"EndEffectorForces", "[", 
        RowBox[{
        "thetalist", ",", "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
        "]"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{
         "2.8225721050069685", ",", "1.530490398292177", ",", 
          "1.6826198448603173"}], "}"}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"EndEffectorForces", "[", 
     RowBox[{
     "thetalist_", ",", "Ftip_", ",", "Mlist_", ",", "Glist_", ",", 
      "Slist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "JTFtip", ",", "n", ",", "dthetalist", ",", "ddthetalist", ",", "g"}], 
       "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "Slist", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"dthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"ddthetalist", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "n"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"g", " ", "=", " ", 
        RowBox[{"ConstantArray", "[", 
         RowBox[{"0", ",", "3"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"JTFtip", " ", "=", " ", 
        RowBox[{"InverseDynamics", "[", 
         RowBox[{
         "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "g", ",", 
          "Ftip", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "JTFtip", "]"}], ";"}]}], "\[IndentingNewLine]",
      "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.677319029687563*^9, 3.677319233082024*^9}, {
   3.677319283141276*^9, 3.677319344821434*^9}, {3.6774904635778027`*^9, 
   3.677490465727865*^9}, 3.678096352332324*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"ForwardDynamics", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "taulist_", ",", "g_", ",", 
       "Ftip_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "variables"}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"A", " ", "list", " ", "of", " ", "joint", " ", "rates"}]}], ",",
     "\[IndentingNewLine]", 
    RowBox[{"taulist", ":", 
     RowBox[{
      RowBox[{"An", " ", "n"}], "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", 
       RowBox[{"forces", "/", "torques"}]}]}]}], ",", 
    RowBox[{"g", ":", 
     RowBox[{"Gravity", " ", "vector", " ", "g"}]}], ",", 
    RowBox[{"Ftip", ":", 
     RowBox[{
      RowBox[{
      "Spatial", " ", "force", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", "expressed", " ", "in", " ", "frame", " ", 
       RowBox[{"{", 
        RowBox[{"n", "+", "1"}], "}"}]}]}]}], ",", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{"Slist", ":", 
       RowBox[{
       "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
        " ", "in", " ", "a", " ", "space", " ", 
        RowBox[{
        "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
         "Returns"}], " ", "ddthetalist"}], ":", 
       RowBox[{"The", " ", "resulting", " ", "joint", " ", 
        RowBox[{"accelerations", ".", "\[IndentingNewLine]", "This"}], " ", 
        "function", " ", "computes", " ", "ddthetalist", " ", "by", " ", 
        RowBox[{"solving", ":", "\[IndentingNewLine]", 
         RowBox[{"Mlist", 
          RowBox[{"(", "thetalist", ")"}], "ddthetalist"}]}]}]}], "=", 
      RowBox[{"taulist", "-", 
       RowBox[{"c", 
        RowBox[{"(", 
         RowBox[{"thetalist", ",", "dthetalist"}], ")"}]}], "-", 
       RowBox[{"g", 
        RowBox[{"(", "thetalist", ")"}]}], "-", 
       RowBox[{"Jtr", 
        RowBox[{"(", "thetalist", ")"}], "Ftip"}]}]}], "\[IndentingNewLine]", 
     ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Input", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"taulist", "=", 
      RowBox[{"{", 
       RowBox[{"0.5", ",", "0.6", ",", "0.7"}], "}"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Ftip", "=", 
      RowBox[{"{", 
       RowBox[{"1", ",", "1", ",", "1", ",", "1", ",", "1", ",", "1"}], 
       "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"ddthetalist", " ", "=", " ", 
      RowBox[{
       RowBox[{"ForwardDynamics", "[", 
        RowBox[{
        "thetalist", ",", "dthetalist", ",", "taulist", ",", "g", ",", "Ftip",
          ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}], 
       "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"-", "17.328839356076827"}], ",", "20.994561292111552", ",",
           "10.365077142755759"}], "}"}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"ForwardDynamics", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "taulist_", ",", "g_", ",", 
      "Ftip_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"ddthetalist", ",", "RHS", ",", "InMatTrans"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"RHS", "=", 
        RowBox[{"taulist", " ", "-", " ", 
         RowBox[{"VelQuadraticForces", "[", 
          RowBox[{
          "thetalist", ",", "dthetalist", ",", "Mlist", ",", "Glist", ",", 
           "Slist"}], "]"}], "-", 
         RowBox[{"GravityForces", "[", 
          RowBox[{
          "thetalist", ",", "g", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
          "]"}], "-", 
         RowBox[{"EndEffectorForces", "[", 
          RowBox[{
          "thetalist", ",", "Ftip", ",", "Mlist", ",", "Glist", ",", 
           "Slist"}], "]"}]}]}], ";", "\n", 
       RowBox[{"InMatTrans", "=", 
        RowBox[{
         RowBox[{"MassMatrix", "[", 
          RowBox[{"thetalist", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
          "]"}], "\[Transpose]"}]}], ";", "\n", 
       RowBox[{"ddthetalist", "=", 
        RowBox[{"(", 
         RowBox[{"InMatTrans", ".", "RHS"}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "ddthetalist", "]"}], ";"}]}], 
     "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.677319777222876*^9, 3.677319911210836*^9}, {
   3.677319978967498*^9, 3.6773200301939793`*^9}, {3.677320101176361*^9, 
   3.677320165996676*^9}, {3.677320275527318*^9, 3.677320277148011*^9}, {
   3.6773204032483463`*^9, 3.677320433741617*^9}, {3.67732052567733*^9, 
   3.677320526931965*^9}, {3.677320560922406*^9, 3.6773205774510202`*^9}, {
   3.6773206325151443`*^9, 3.677320917826007*^9}, {3.677320961401045*^9, 
   3.6773210650000067`*^9}, {3.67749047640164*^9, 3.677490476845508*^9}, 
   3.678096359107258*^9, 3.67844919580091*^9, 3.681451022756323*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"EulerStep", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "ddthetalist_", ",", "dt_"}], 
      "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "variables"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "rates"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"ddthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "accelerations"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"dt", ":", 
     RowBox[{
     "The", " ", "timestep", " ", "delta", " ", "t", "\n", 
      "\[IndentingNewLine]", "Returns", " ", "thetalistNext"}], ":", 
     RowBox[{
     "Vector", " ", "of", " ", "joint", " ", "variables", " ", "after", " ", 
      "dt", " ", "from", " ", "first", " ", "order", " ", "Euler", " ", 
      "integration"}]}], ",", 
    RowBox[{
     RowBox[{"dthetalistNext", ":", 
      RowBox[{
      "Vector", " ", "of", " ", "joint", " ", "rates", " ", "after", " ", 
       "dt", " ", "from", " ", "first", " ", "order", " ", "Euler", " ", 
       RowBox[{"integration", "."}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{"Example", " ", "Inputs", " ", 
        RowBox[{"(", 
         RowBox[{"3", " ", "Link", " ", "Robot"}], ")"}]}], ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", 
     RowBox[{"ddthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"2", ",", "1.5", ",", "1"}], "}"}]}], ";", "\n", 
     RowBox[{"dt", "=", "0.1"}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"thetalistNext", ",", "dthetalistNext"}], "}"}], " ", "=", " ", 
      RowBox[{
       RowBox[{"EulerStep", "[", 
        RowBox[{
        "thetalist", ",", "dthetalist", ",", "ddthetalist", ",", "dt"}], 
        "]"}], "\n", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"0.11", ",", "0.12", ",", "0.13"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"0.3", ",", "0.35", ",", "0.4"}], "}"}]}], "}"}]}]}]}]}]}],
    "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"EulerStep", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "ddthetalist_", ",", "dt_"}], 
     "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"thetalistNext", ",", "dthetalistNext"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"thetalistNext", " ", "=", " ", 
        RowBox[{"thetalist", " ", "+", " ", 
         RowBox[{"dt", "*", "dthetalist"}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"dthetalistNext", " ", "=", " ", 
        RowBox[{"dthetalist", " ", "+", " ", 
         RowBox[{"dt", "*", "ddthetalist"}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", 
        RowBox[{"{", 
         RowBox[{"thetalistNext", ",", "dthetalistNext"}], "}"}], "]"}], 
       ";"}]}], "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.6773212430112534`*^9, 
   3.6773213088789883`*^9}, {3.677321361759098*^9, 3.677321367090461*^9}, {
   3.677321470715757*^9, 3.6773215552292967`*^9}, {3.677321669275291*^9, 
   3.677321799799973*^9}, {3.6773218479713087`*^9, 3.6773219232750673`*^9}, 
   3.6774148217031*^9, 3.677414865205659*^9, 3.6780963661744823`*^9}],

Cell[BoxData[
 RowBox[{"\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"InverseDynamicsTrajectory", "[", 
      RowBox[{
      "thetamat_", ",", "dthetamat_", ",", "ddthetamat_", ",", "g_", ",", 
       "Ftipmat_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":", 
     "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetamat"}], ":", 
     RowBox[{
     "An", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "robot", " ", "joint", " ", "variables"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"dthetamat", ":", 
     RowBox[{
     "An", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "robot", " ", "joint", " ", "velocities"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"ddthetamat", ":", 
     RowBox[{
     "An", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "robot", " ", "joint", " ", "accelerations"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"Gravity", " ", "vector", " ", "g"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Ftipmat", ":", 
     RowBox[{
      RowBox[{
      "An", " ", "N", " ", "x", " ", "6", " ", "matrix", " ", "of", " ", 
       "spatial", " ", "forces", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", 
       RowBox[{"(", 
        RowBox[{
        "If", " ", "there", " ", "are", " ", "no", " ", "tip", " ", "forces", 
         " ", "the", " ", "user", " ", "should", " ", "input", " ", "a", " ", 
         "zero", " ", "and", " ", "a", " ", "zero", " ", "matrix", " ", 
         "will", " ", "be", " ", "used"}], ")"}]}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "List", " ", "of", " ", "link", " ", "frames", " ", "i", " ", 
       "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
      " ", "in", " ", "a", " ", "space", " ", 
      RowBox[{
      "frame", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}],
       " ", "taumat"}], ":", 
     RowBox[{
     "The", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "joint", " ", 
      RowBox[{"forces", "/", "torques"}], " ", "for", " ", "the", " ", 
      "specified", " ", "trajectory"}]}], ",", 
    RowBox[{
     RowBox[{
     "where", " ", "each", " ", "of", "\n", "the", " ", "N", " ", "rows", " ",
       "is", " ", "the", " ", "vector", " ", "of", " ", "joint", " ", 
      RowBox[{"forces", "/", "torques"}], " ", "at", " ", "each", " ", "time",
       " ", 
      RowBox[{"step", ".", "\[IndentingNewLine]", "This"}], " ", "function", 
      " ", "uses", " ", "InverseDynamics", " ", "to", " ", "calculate", " ", 
      "the", " ", "joint", " ", 
      RowBox[{"forces", "/", "torques"}], " ", "required", " ", "to", " ", 
      "move", " ", "the", " ", "serial", " ", "chain", " ", "along", " ", 
      "the", " ", "given", " ", 
      RowBox[{"trajectory", "."}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{"Example", " ", 
      RowBox[{"Input", ":", "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
        "Create", " ", "a", " ", "trajectory", " ", "to", " ", "follow", " ", 
         "using", " ", "functions", " ", "from", " ", "Chapter", " ", "9"}], 
        "*)"}], "\n", 
       RowBox[{"thetastart", "=", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}]}]}], ";", "\n", 
     RowBox[{"thetaend", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Pi", "/", "2"}], ",", 
        RowBox[{"Pi", "/", "2"}], ",", 
        RowBox[{"Pi", "/", "2"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Tf", "=", "3"}], ";", "\n", 
     RowBox[{"No", "=", "1000"}], ";", "\n", 
     RowBox[{"method", "=", "5"}], ";", "\n", 
     RowBox[{"traj", "=", 
      RowBox[{"JointTrajectory", "[", 
       RowBox[{
       "thetastart", ",", "thetaend", ",", "Tf", ",", "No", ",", "method"}], 
       "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"thetamat", "=", 
      RowBox[{"{", 
       RowBox[{"traj", "[", 
        RowBox[{"[", "1", "]"}], "]"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetamat", "=", 
      RowBox[{"{", 
       RowBox[{"thetamat", "[", 
        RowBox[{"[", "1", "]"}], "]"}], "}"}]}], ";", "\n", 
     RowBox[{"ddthetamat", "=", 
      RowBox[{"{", 
       RowBox[{"dthetamat", "[", 
        RowBox[{"[", "1", "]"}], "]"}], "}"}]}], ";", "\n", 
     RowBox[{"dt", "=", 
      RowBox[{"Tf", "/", 
       RowBox[{"(", 
        RowBox[{"No", "-", "1.0"}], ")"}]}]}], ";", "\n", 
     RowBox[{"Do", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"AppendTo", "[", 
         RowBox[{"thetamat", ",", 
          RowBox[{"traj", "[", 
           RowBox[{"[", "i", "]"}], "]"}]}], "]"}], ";", "\n", 
        RowBox[{"AppendTo", "[", 
         RowBox[{"dthetamat", ",", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"thetamat", "[", 
              RowBox[{"[", "i", "]"}], "]"}], "-", 
             RowBox[{"thetamat", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "1"}], "]"}], "]"}]}], ")"}], "/", "dt"}]}], 
         "]"}], ";", "\n", 
        RowBox[{"AppendTo", "[", 
         RowBox[{"ddthetamat", ",", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"dthetamat", "[", 
              RowBox[{"[", "i", "]"}], "]"}], "-", 
             RowBox[{"dthetamat", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "1"}], "]"}], "]"}]}], ")"}], "/", "dt"}]}], 
         "]"}], ";"}], "\[IndentingNewLine]", ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "2", ",", 
         RowBox[{"Length", "[", "traj", "]"}]}], "}"}]}], "]"}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Initialise", " ", "robot", " ", "descripstion", " ", 
       RowBox[{"(", 
        RowBox[{"Example", " ", "with", " ", "3", " ", "links"}], ")"}]}], 
      "*)"}], "\[IndentingNewLine]", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Ftipmat", "=", 
      RowBox[{"ConstantArray", "[", 
       RowBox[{"1", ",", 
        RowBox[{"{", 
         RowBox[{"No", ",", "6"}], "}"}]}], "]"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "0.23", ",", "0.1"}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "1.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0.2"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"taumat", "=", 
      RowBox[{"InverseDynamicsTrajectory", "[", 
       RowBox[{
       "thetamat", ",", "dthetamat", ",", "ddthetamat", ",", "g", ",", 
        "Ftipmat", ",", "Mlist", ",", "Glist", ",", "Slist"}], "]"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Output", " ", "to", " ", "plot", " ", "the", " ", "joint", " ", 
       RowBox[{"forces", "/", "torques"}]}], "*)"}], "\[IndentingNewLine]", 
     RowBox[{"Tau1", " ", "=", " ", 
      RowBox[{"taumat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "1"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Tau2", " ", "=", " ", 
      RowBox[{"taumat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "2"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Tau3", " ", "=", " ", 
      RowBox[{"taumat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "3"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]",
      "\[IndentingNewLine]", 
     RowBox[{"ListLinePlot", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"Tau1", ",", "Tau2", ",", "Tau3"}], "}"}], ",", 
       RowBox[{"PlotLegends", "\[Rule]", 
        RowBox[{"{", 
         RowBox[{"\"\<Tau1\>\"", ",", "\"\<Tau2\>\"", ",", "\"\<Tau3\>\""}], 
         "}"}]}], ",", 
       RowBox[{"AxesLabel", "\[Rule]", 
        RowBox[{"{", 
         RowBox[{"\"\<Time\>\"", ",", "\"\<Torque\>\""}], "}"}]}], ",", 
       RowBox[{
       "PlotLabel", "\[Rule]", "\"\<Plot of Torque Trajectories\>\""}], ",", 
       RowBox[{"PlotRange", "\[Rule]", "Full"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"InverseDynamicsTrajectory", "[", 
     RowBox[{
     "thetamat_", ",", "dthetamat_", ",", "ddthetamat_", ",", "g_", ",", 
      "Ftipmat_", ",", "Mlist_", ",", "Glist_", ",", "Slist_"}], "]"}], ":=", 
    
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"taumat", ",", "n", ",", "NewFtipmat"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "thetamat", "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"NewFtipmat", " ", "=", " ", "Ftipmat"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"If", "[", 
        RowBox[{
         RowBox[{"Ftipmat", " ", "\[Equal]", " ", "0"}], ",", " ", 
         RowBox[{"NewFtipmat", " ", "=", " ", 
          RowBox[{"ConstantArray", "[", 
           RowBox[{"0", ",", 
            RowBox[{"{", 
             RowBox[{
              RowBox[{"Length", "[", "thetamat", "]"}], ",", "6"}], "}"}]}], 
           "]"}]}]}], "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"taumat", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", "\[IndentingNewLine]", 
        RowBox[{
         RowBox[{
          RowBox[{"AppendTo", "[", 
           RowBox[{"taumat", ",", 
            RowBox[{"InverseDynamics", "[", 
             RowBox[{
              RowBox[{"thetamat", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", 
              RowBox[{"dthetamat", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", 
              RowBox[{"ddthetamat", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", "g", ",", 
              RowBox[{"NewFtipmat", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", "Mlist", ",", "Glist", 
              ",", "Slist"}], "]"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
         ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "taumat", "]"}], ";"}]}], "\[IndentingNewLine]",
      "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.67732315475941*^9, 3.677323235084415*^9}, {
   3.677323276630302*^9, 3.677323434547833*^9}, {3.677323484461143*^9, 
   3.677323499819213*^9}, {3.677323535146171*^9, 3.6773235769457493`*^9}, {
   3.677323628046247*^9, 3.6773236934411507`*^9}, {3.677327123605197*^9, 
   3.6773271239634647`*^9}, {3.677328059209757*^9, 3.6773280603224277`*^9}, {
   3.677331788835787*^9, 3.677331816859844*^9}, {3.677332081788471*^9, 
   3.677332105218265*^9}, {3.677333800582396*^9, 3.67733380225889*^9}, {
   3.6773420774064093`*^9, 3.6773420779146147`*^9}, {3.677405584192568*^9, 
   3.677405597246728*^9}, {3.677415011086073*^9, 3.6774150158485613`*^9}, {
   3.677417169219336*^9, 3.677417179691127*^9}, {3.6774904905071697`*^9, 
   3.677490490950142*^9}, {3.67809637333982*^9, 3.6780963899455423`*^9}, {
   3.678452980732629*^9, 3.678453008727953*^9}, {3.678455504215991*^9, 
   3.678455559017528*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"ForwardDynamicsTrajectory", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "taumat_", ",", "g_", ",", 
       "Ftipmat_", ",", "Mlist_", ",", "Glist_", ",", "Slist_", ",", "dt_", 
       ",", "intRes_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "initial", " ", "joint", " ", 
       "variables"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "initial", " ", "joint", " ", "rates"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"taumat", ":", 
     RowBox[{
     "An", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "joint", " ", 
      RowBox[{"forces", "/", "torques"}]}]}], ",", 
    RowBox[{
    "where", " ", "each", " ", "row", " ", "is", " ", "the", " ", "joint", 
     " ", "effort", " ", "at", " ", "any", " ", "time", " ", "step"}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"Gravity", " ", "vector", " ", "g"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Ftipmat", ":", 
     RowBox[{
      RowBox[{
      "An", " ", "N", " ", "x", " ", "6", " ", "matrix", " ", "of", " ", 
       "spatial", " ", "forces", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", 
       RowBox[{"(", 
        RowBox[{
        "If", " ", "there", " ", "are", " ", "no", " ", "tip", " ", "forces", 
         " ", "the", " ", "user", " ", "should", " ", "input", " ", "a", " ", 
         "zero", " ", "and", " ", "a", " ", "zero", " ", "matrix", " ", 
         "will", " ", "be", " ", "used"}], ")"}]}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Mlist", ":", 
     RowBox[{"List", " ", "of", " ", "link", " ", "frames", " ", 
      RowBox[{"{", "i", "}"}], " ", "relative", " ", "to", " ", 
      RowBox[{"{", 
       RowBox[{"i", "-", "1"}], "}"}], " ", "at", " ", "the", " ", "home", 
      " ", "position"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
      " ", "in", " ", "a", " ", "space", " ", "frame"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"dt", ":", 
     RowBox[{
     "The", " ", "timestep", " ", "between", " ", "consecutive", " ", "joint",
       " ", 
      RowBox[{"forces", "/", "torques"}]}]}], ",", 
    RowBox[{"intRes", ":", 
     RowBox[{
     "Integration", " ", "resolution", " ", "is", " ", "the", " ", "number", 
      " ", "of", " ", "times", " ", "integration", " ", 
      RowBox[{"(", "Euler", ")"}], " ", "takes", " ", "places", " ", 
      "between", " ", "each", " ", "time", " ", 
      RowBox[{"step", ".", " ", "Must"}], " ", "be", " ", "an", " ", 
      "integer", " ", "value", " ", "greater", " ", "than", " ", "or", " ", 
      "equal", " ", "to", " ", "1", "\n", "\[IndentingNewLine]", "Returns", 
      " ", "thetamat"}], ":", 
     RowBox[{
     "The", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
      "robot", " ", "joint", " ", "angles", " ", "resulting", " ", "from", 
      " ", "the", "\n", "specified", " ", "joint", " ", 
      RowBox[{"forces", "/", "torques"}]}]}], ",", 
    RowBox[{
     RowBox[{"dthetamat", ":", 
      RowBox[{
       RowBox[{
       "The", " ", "N", " ", "x", " ", "n", " ", "matrix", " ", "of", " ", 
        "robot", " ", "joint", " ", 
        RowBox[{"velocities", ".", "\[IndentingNewLine]", "This"}], " ", 
        "function", " ", "simulates", " ", "the", " ", "motion", " ", "of", 
        " ", "a", " ", "serial", " ", "chain", " ", "given", " ", "an", " ", 
        "open"}], "-", 
       RowBox[{"loop", " ", "history", "\n", "of", " ", "joint", " ", 
        RowBox[{"forces", "/", 
         RowBox[{"torques", ".", " ", "It"}]}], " ", "calls", " ", "a", " ", 
        "numerical", " ", "integration", " ", "procedure", " ", "that", " ", 
        "uses", " ", 
        RowBox[{"ForwardDynamics", "."}]}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"Example", " ", 
       RowBox[{"Inputs", ":", "\n", "thetalist"}]}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", 
     RowBox[{"taumat", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"3.63", ",", 
          RowBox[{"-", "6.58"}], ",", 
          RowBox[{"-", "5.57"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"3.74", ",", 
          RowBox[{"-", "5.55"}], ",", 
          RowBox[{"-", "5.5"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"4.31", ",", 
          RowBox[{"-", "0.68"}], ",", 
          RowBox[{"-", "5.19"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"5.18", ",", "5.63", ",", 
          RowBox[{"-", "4.31"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"5.85", ",", "8.17", ",", 
          RowBox[{"-", "2.59"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"5.78", ",", "2.79", ",", 
          RowBox[{"-", "1.7"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"4.99", ",", 
          RowBox[{"-", "5.3"}], ",", 
          RowBox[{"-", "1.19"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"4.08", ",", 
          RowBox[{"-", "9.41"}], ",", "0.07"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"3.56", ",", 
          RowBox[{"-", "10.1"}], ",", "0.97"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"3.49", ",", 
          RowBox[{"-", "9.41"}], ",", "1.23"}], "}"}]}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Initialise", " ", "robot", " ", "descripstion", " ", 
       RowBox[{"(", 
        RowBox[{"Example", " ", "with", " ", "3", " ", "links"}], ")"}]}], 
      "*)"}], "\n", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Ftipmat", "=", 
      RowBox[{"ConstantArray", "[", 
       RowBox[{"1", ",", 
        RowBox[{"{", 
         RowBox[{"No", ",", "6"}], "}"}]}], "]"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"dt", "=", "0.1"}], ";", "\n", 
     RowBox[{"intRes", "=", "8"}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"thetamat", ",", "dthetamat"}], "}"}], "=", 
      RowBox[{"ForwardDynamicsTrajectory", "[", 
       RowBox[{
       "thetalist", ",", "dthetalist", ",", "taumat", ",", "g", ",", 
        "Ftipmat", ",", "Mlist", ",", "Glist", ",", "Slist", ",", "dt", ",", 
        "intRes"}], "]"}]}], ";", "\[IndentingNewLine]", 
     "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Output", " ", "to", " ", "plot", " ", "the", " ", "joint", " ", 
       RowBox[{"forces", "/", "torques"}]}], "*)"}], "\[IndentingNewLine]", 
     RowBox[{"theta1", " ", "=", " ", 
      RowBox[{"thetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "1"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"theta2", " ", "=", " ", 
      RowBox[{"thetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "2"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"theta3", " ", "=", " ", 
      RowBox[{"thetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "3"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"dtheta1", " ", "=", " ", 
      RowBox[{"dthetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "1"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"dtheta2", " ", "=", " ", 
      RowBox[{"dthetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "2"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"dtheta3", " ", "=", " ", 
      RowBox[{"dthetamat", "[", 
       RowBox[{"[", 
        RowBox[{"All", ",", "3"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]",
      "\[IndentingNewLine]", 
     RowBox[{"ListLinePlot", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "theta1", ",", "theta2", ",", "theta3", ",", "dtheta1", ",", 
         "dtheta2", ",", "dtheta3"}], "}"}], ",", 
       RowBox[{"PlotLegends", "\[Rule]", 
        RowBox[{"{", 
         RowBox[{
         "\"\<theta1\>\"", ",", "\"\<theta2\>\"", ",", "\"\<theta3\>\"", ",", 
          "\"\<dtheta1\>\"", ",", "\"\<dtheta2\>\"", ",", "\"\<dtheta3\>\""}],
          "}"}]}], ",", 
       RowBox[{"AxesLabel", "\[Rule]", 
        RowBox[{"{", 
         RowBox[{"\"\<Time\>\"", ",", "\"\<Joint Angles/Velocities\>\""}], 
         "}"}]}], ",", 
       RowBox[{
       "PlotLabel", "\[Rule]", 
        "\"\<Plot of Joint Angles and Joint Velocities\>\""}], ",", 
       RowBox[{"PlotRange", "\[Rule]", "Full"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"ForwardDynamicsTrajectory", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "taumat_", ",", "g_", ",", 
      "Ftipmat_", ",", "Mlist_", ",", "Glist_", ",", "Slist_", ",", "dt_", 
      ",", "intRes_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "n", ",", "thetamat", ",", "dthetamat", ",", "ddthetalist", ",", 
        "thetanext", ",", "dthetanext", ",", "NewFtipmat"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", " ", "=", " ", 
        RowBox[{"Length", "[", "taumat", "]"}]}], ";", "\[IndentingNewLine]", 
       
       RowBox[{"thetamat", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"NewFtipmat", " ", "=", " ", "Ftipmat"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"If", "[", 
        RowBox[{
         RowBox[{"Ftipmat", " ", "\[Equal]", " ", "0"}], ",", " ", 
         RowBox[{"NewFtipmat", " ", "=", " ", 
          RowBox[{"ConstantArray", "[", 
           RowBox[{"0", ",", 
            RowBox[{"{", 
             RowBox[{
              RowBox[{"Length", "[", "taumat", "]"}], ",", "6"}], "}"}]}], 
           "]"}]}]}], "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"AppendTo", "[", 
        RowBox[{"thetamat", ",", " ", "thetalist"}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"dthetamat", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"AppendTo", "[", 
        RowBox[{"dthetamat", ",", " ", "dthetalist"}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"thetanext", " ", "=", " ", "thetalist"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"dthetanext", " ", "=", " ", "dthetalist"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Do", "[", "\[IndentingNewLine]", 
        RowBox[{
         RowBox[{
          RowBox[{"Do", "[", "\[IndentingNewLine]", 
           RowBox[{
            RowBox[{
             RowBox[{"ddthetalist", "=", 
              RowBox[{"ForwardDynamics", "[", 
               RowBox[{"thetanext", ",", "dthetanext", ",", 
                RowBox[{"taumat", "[", 
                 RowBox[{"[", "i", "]"}], "]"}], ",", "g", ",", 
                RowBox[{"NewFtipmat", "[", 
                 RowBox[{"[", "i", "]"}], "]"}], ",", "Mlist", ",", "Glist", 
                ",", "Slist"}], "]"}]}], ";", "\[IndentingNewLine]", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"thetanext", ",", "dthetanext"}], "}"}], "=", 
              RowBox[{"EulerStep", "[", 
               RowBox[{
               "thetanext", ",", "dthetanext", ",", "ddthetalist", ",", 
                RowBox[{"(", 
                 RowBox[{"dt", "/", "intRes"}], ")"}]}], "]"}]}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"AppendTo", "[", 
              RowBox[{"thetamat", ",", " ", "thetanext"}], "]"}], ";", 
             "\[IndentingNewLine]", 
             RowBox[{"AppendTo", "[", 
              RowBox[{"dthetamat", ",", " ", "dthetanext"}], "]"}], ";"}], 
            "\[IndentingNewLine]", ",", 
            RowBox[{"{", 
             RowBox[{"j", ",", "1", ",", "intRes"}], "}"}]}], "]"}], ";"}], 
         "\[IndentingNewLine]", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", 
        RowBox[{"{", 
         RowBox[{"thetamat", ",", "dthetamat"}], "}"}], "]"}], ";"}]}], 
     "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.658273872689763*^9, 3.658273988902459*^9}, {
   3.658274157743518*^9, 3.6582741742369556`*^9}, {3.65827421723704*^9, 
   3.6582742187327127`*^9}, {3.6582742677636137`*^9, 3.658274330534523*^9}, {
   3.658274365312026*^9, 3.6582744138260403`*^9}, {3.658274810942898*^9, 
   3.658274859112701*^9}, {3.658275033847995*^9, 3.658275109340135*^9}, {
   3.658275161547172*^9, 3.6582752536631193`*^9}, {3.65827533442001*^9, 
   3.658275354865328*^9}, {3.6582755590331087`*^9, 3.658275644746291*^9}, {
   3.658275839400229*^9, 3.6582758455265408`*^9}, {3.6582758973266172`*^9, 
   3.658276032713943*^9}, {3.658276065782716*^9, 3.6582762151364822`*^9}, {
   3.658276271302123*^9, 3.6582763371711683`*^9}, 3.658276384203477*^9, {
   3.658276429293391*^9, 3.658276729581854*^9}, {3.658276847155965*^9, 
   3.6582768822910967`*^9}, 3.658277078361019*^9, {3.658277454514134*^9, 
   3.65827755246841*^9}, {3.658277707616706*^9, 3.658277724793824*^9}, {
   3.658277775557291*^9, 3.658277790046358*^9}, {3.6582778408966618`*^9, 
   3.658277880321537*^9}, {3.658277965874803*^9, 3.658278001723259*^9}, {
   3.658278068864526*^9, 3.658278122465324*^9}, {3.6582782743035307`*^9, 
   3.658278463989525*^9}, {3.658278495324057*^9, 3.6582785116036882`*^9}, {
   3.6582787258401527`*^9, 3.6582787620388193`*^9}, {3.658278813660791*^9, 
   3.658278890779448*^9}, {3.658278981426128*^9, 3.6582789851507807`*^9}, {
   3.658279023852993*^9, 3.658279071286935*^9}, {3.6582792891814613`*^9, 
   3.6582793220434237`*^9}, {3.658279596716886*^9, 3.658279661442956*^9}, {
   3.658279700093648*^9, 3.658279706815292*^9}, {3.658279792551382*^9, 
   3.658279815830398*^9}, {3.658279878622261*^9, 3.658279897713276*^9}, {
   3.658279959320986*^9, 3.658279968167665*^9}, {3.658280159539792*^9, 
   3.658280259153908*^9}, {3.658280389314715*^9, 3.658280492142959*^9}, {
   3.65828054504196*^9, 3.6582806104625673`*^9}, {3.658280703485147*^9, 
   3.6582807265634003`*^9}, {3.658280783403376*^9, 3.658280995068233*^9}, {
   3.658332125643222*^9, 3.658332285669385*^9}, {3.658332356434971*^9, 
   3.658332432532056*^9}, {3.6583324694681797`*^9, 3.658332536889868*^9}, {
   3.6583325787135487`*^9, 3.658332603918232*^9}, {3.658332777986459*^9, 
   3.658332831919935*^9}, {3.6583329211917048`*^9, 3.6583329244763107`*^9}, {
   3.65833298690902*^9, 3.6583329940420103`*^9}, {3.658333033513297*^9, 
   3.658333041965459*^9}, {3.658333140934991*^9, 3.6583331410849524`*^9}, {
   3.658333260874716*^9, 3.658333325111835*^9}, {3.6583333640450163`*^9, 
   3.658333602194475*^9}, {3.658333866428316*^9, 3.658333876484424*^9}, {
   3.6583340410637217`*^9, 3.658334041446103*^9}, {3.658334239767832*^9, 
   3.658334256853527*^9}, {3.658334325290709*^9, 3.658334328931733*^9}, {
   3.658334454601124*^9, 3.658334541125682*^9}, {3.658334583676895*^9, 
   3.658334664745475*^9}, {3.658334705147303*^9, 3.6583347127203903`*^9}, {
   3.658334814800128*^9, 3.658334905957917*^9}, {3.658334952676396*^9, 
   3.658334989092309*^9}, {3.658335053255745*^9, 3.658335071690804*^9}, {
   3.658335129550527*^9, 3.658335241594574*^9}, {3.6583353167377977`*^9, 
   3.658335317096506*^9}, {3.658335384569755*^9, 3.658335396679709*^9}, {
   3.658335459752262*^9, 3.658335474398382*^9}, {3.658335569564763*^9, 
   3.6583356135466957`*^9}, {3.658335963702228*^9, 3.6583360118223963`*^9}, {
   3.6583361961257257`*^9, 3.658336324493081*^9}, {3.658336387471901*^9, 
   3.658336413286467*^9}, {3.6583364883081408`*^9, 3.658336518027725*^9}, {
   3.6583497860827847`*^9, 3.658349831490996*^9}, {3.658349861737459*^9, 
   3.658349862153501*^9}, {3.658349942786976*^9, 3.658349965329233*^9}, {
   3.6583500320501423`*^9, 3.658350038684167*^9}, {3.6583500726428823`*^9, 
   3.658350074339263*^9}, {3.658350117777423*^9, 3.658350161383792*^9}, {
   3.6583501955208597`*^9, 3.658350234542882*^9}, {3.6583502755982323`*^9, 
   3.658350338283965*^9}, {3.658350380319613*^9, 3.6583503814310093`*^9}, {
   3.658350450869604*^9, 3.658350549946392*^9}, {3.658350685268361*^9, 
   3.6583506855161467`*^9}, {3.658350727058009*^9, 3.6583507290251617`*^9}, {
   3.658350771093258*^9, 3.658350847603608*^9}, {3.658350951089933*^9, 
   3.658350961855956*^9}, {3.658351021399521*^9, 3.658351021591137*^9}, {
   3.6583511160719757`*^9, 3.658351121390588*^9}, {3.658351310629883*^9, 
   3.658351330581682*^9}, {3.658351453089098*^9, 3.6583514579767447`*^9}, {
   3.6583514969183617`*^9, 3.6583514999993277`*^9}, {3.658351570635014*^9, 
   3.658351577593944*^9}, {3.6583516106406183`*^9, 3.658351657352182*^9}, {
   3.658351766088022*^9, 3.658351770343424*^9}, {3.658351890506502*^9, 
   3.658351904164137*^9}, {3.6583519453391848`*^9, 3.65835200222512*^9}, {
   3.658352444511814*^9, 3.6583524450885687`*^9}, {3.65835945210156*^9, 
   3.658359582160754*^9}, {3.6583597573611526`*^9, 3.658359895421398*^9}, {
   3.6583600200459547`*^9, 3.658360209087796*^9}, {3.6583604062023983`*^9, 
   3.658360556362865*^9}, {3.658369386717368*^9, 3.658369415920825*^9}, {
   3.658369495583129*^9, 3.658369511397794*^9}, {3.658369604058823*^9, 
   3.6583698687217712`*^9}, {3.658369905522255*^9, 3.658370289459115*^9}, {
   3.658435473074052*^9, 3.65843549562414*^9}, {3.658435606368175*^9, 
   3.658435611544252*^9}, {3.6584356461976643`*^9, 3.6584356582216187`*^9}, {
   3.65843569293461*^9, 3.658435693127248*^9}, {3.6584357386455507`*^9, 
   3.6584357504379663`*^9}, {3.658437328158703*^9, 3.658437328842226*^9}, {
   3.658451386320923*^9, 3.658451396416233*^9}, {3.6584514558014927`*^9, 
   3.658451498437763*^9}, {3.65845176533173*^9, 3.658451769500102*^9}, {
   3.658451827834752*^9, 3.658451828192766*^9}, {3.6584518628797626`*^9, 
   3.658451915580929*^9}, {3.658451961311802*^9, 3.658451961463069*^9}, {
   3.658452686235773*^9, 3.658452689778323*^9}, {3.6584528065184526`*^9, 
   3.658452806708708*^9}, {3.6586220651258087`*^9, 3.658622180449822*^9}, {
   3.658622217313324*^9, 3.658622381467565*^9}, {3.658622437008402*^9, 
   3.658622500999549*^9}, {3.658623683248323*^9, 3.6586241287979918`*^9}, {
   3.658624240472498*^9, 3.658624332091518*^9}, {3.6586243934910803`*^9, 
   3.6586246606532173`*^9}, {3.677407880850321*^9, 3.6774079492889233`*^9}, 
   3.677414022557517*^9, {3.6774140684385967`*^9, 3.677414075630186*^9}, {
   3.677414335095175*^9, 3.6774144298603773`*^9}, {3.677414470492803*^9, 
   3.6774146091155233`*^9}, {3.67741468942129*^9, 3.6774147932681417`*^9}, {
   3.67741491149364*^9, 3.677414922353374*^9}, {3.67741498492174*^9, 
   3.6774151635455713`*^9}, {3.677415203976342*^9, 3.677415224900571*^9}, {
   3.6774152627756023`*^9, 3.6774153898486433`*^9}, {3.677415447679756*^9, 
   3.677415479578895*^9}, {3.677415510129551*^9, 3.6774155189848757`*^9}, {
   3.677415563853796*^9, 3.677415658868799*^9}, {3.6774156922934237`*^9, 
   3.677415750725058*^9}, 3.677415816213516*^9, 3.677415995234165*^9, {
   3.677416164828886*^9, 3.677416172065155*^9}, 3.6774162123220577`*^9, 
   3.6774162696380577`*^9, {3.677416302219317*^9, 3.6774163049989634`*^9}, {
   3.677416378172185*^9, 3.677416424224967*^9}, {3.677416454762212*^9, 
   3.677416495972261*^9}, {3.6774165285301943`*^9, 3.677416609497998*^9}, {
   3.6774167079897223`*^9, 3.677416809355668*^9}, {3.677417011496028*^9, 
   3.6774171391385193`*^9}, 3.677417196932665*^9, {3.6774905052362013`*^9, 
   3.67749050537529*^9}, {3.678096405086987*^9, 3.6780964104728518`*^9}, {
   3.678452887470743*^9, 3.678452964874299*^9}, {3.6784530534763193`*^9, 
   3.678453091013393*^9}, {3.678453126802677*^9, 3.6784531289330797`*^9}, {
   3.678453439478467*^9, 3.678453592963132*^9}, {3.67845373611872*^9, 
   3.678453765205407*^9}, {3.6784538527604933`*^9, 3.678453869490027*^9}, {
   3.6784538995984297`*^9, 3.678453940309951*^9}, {3.6784540141405973`*^9, 
   3.678454036151272*^9}, {3.681455189340788*^9, 3.6814552018036423`*^9}, {
   3.6814552579394627`*^9, 3.681455263407737*^9}, {3.681455819481369*^9, 
   3.681455824984709*^9}}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  9: TRAJECTORY GENERATION",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}, {3.6756821024917507`*^9, 3.6756821283850946`*^9}, {
  3.6757648758196497`*^9, 3.675764892595064*^9}, {3.6767235712710247`*^9, 
  3.676723593007389*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"CubicTimeScaling", "[", 
      RowBox[{"Tf_", ",", "t_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Tf"}], ":", 
     RowBox[{
     "Total", " ", "time", " ", "of", " ", "the", " ", "motion", " ", "in", 
      " ", "seconds", " ", "from", " ", "rest", " ", "to", " ", "rest"}]}], 
    ",", 
    RowBox[{
     RowBox[{"t", ":", 
      RowBox[{
       RowBox[{
       "The", " ", "current", " ", "time", " ", "t", " ", "satisfying", " ", 
        "0"}], "<", "t", "<", 
       RowBox[{
        RowBox[{
        "Tf", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}], 
        " ", "s"}]}], ":", 
      RowBox[{
       RowBox[{"The", " ", "path", " ", "parameter", " ", "s", 
        RowBox[{"(", "t", ")"}], " ", "corresponding", " ", "to", " ", "a", 
        " ", "third"}], "-", 
       RowBox[{
       "order", " ", "polynomial", " ", "motion", " ", "that", " ", "begins", 
        " ", "and", "\n", "ends", " ", "at", " ", "zero", " ", 
        RowBox[{"velocity", "."}]}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Tf"}], "=", "2"}], ";", "\n", 
     RowBox[{"t", "=", "0.6"}], ";", "\[IndentingNewLine]", 
     RowBox[{"s", " ", "=", " ", 
      RowBox[{
       RowBox[{"CubicTimeScaling", "[", 
        RowBox[{"Tf", ",", "t"}], "]"}], "\[IndentingNewLine]", 
       "\[IndentingNewLine]", 
       RowBox[{
       "Output", ":", "\[IndentingNewLine]", "0.21600000000000003"}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"CubicTimeScaling", "[", 
     RowBox[{"Tf_", ",", "t_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"s", ",", "a2", ",", "a3"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{"(*", 
       RowBox[{"a0", " ", "and", " ", "a1", " ", "are", " ", "zero"}], "*)"}],
       "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"a2", "=", 
        RowBox[{"(", 
         RowBox[{"3", "/", 
          RowBox[{"(", 
           RowBox[{"Tf", "^", "2"}], ")"}]}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"a3", "=", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", "2"}], "/", 
          RowBox[{"(", 
           RowBox[{"Tf", "^", "3"}], ")"}]}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"s", "=", 
        RowBox[{
         RowBox[{"a2", "*", 
          RowBox[{"t", "^", "2"}]}], "+", 
         RowBox[{"a3", "*", 
          RowBox[{"t", "^", "3"}]}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "s", "]"}], ";"}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675682824450754*^9, 
   3.6756828836706333`*^9}, {3.6756829858535852`*^9, 3.675683093476144*^9}, {
   3.675683163431617*^9, 3.675683301926485*^9}, {3.6756834458193197`*^9, 
   3.675683520674423*^9}, {3.675683569012762*^9, 3.675683584065415*^9}, 
   3.675683635500771*^9, {3.675684508273862*^9, 3.675684520687911*^9}, {
   3.675764977565001*^9, 3.675764979617189*^9}, {3.6757662451157312`*^9, 
   3.675766254498831*^9}, {3.675766317871736*^9, 3.675766345394107*^9}, {
   3.6757664078898573`*^9, 3.675766419041957*^9}, {3.6757665260531073`*^9, 
   3.6757665599652433`*^9}, {3.675766688881428*^9, 3.675766693414764*^9}, {
   3.675766760364307*^9, 3.675766898736827*^9}, {3.675766931380547*^9, 
   3.675766932915333*^9}, 3.675767022524777*^9, 3.675767053903921*^9, {
   3.675767094663486*^9, 3.675767157353602*^9}, {3.6757672038039017`*^9, 
   3.67576720815028*^9}, {3.675767263478516*^9, 3.675767302851701*^9}, {
   3.6757673469407463`*^9, 3.6757675158730087`*^9}, {3.675767609117689*^9, 
   3.6757676149377613`*^9}, 3.676110621848937*^9, {3.676110673192341*^9, 
   3.676110708132433*^9}, {3.676110758266004*^9, 3.676110779724792*^9}, {
   3.676110837502541*^9, 3.676110924302658*^9}, {3.676111015370205*^9, 
   3.676111023652515*^9}, {3.676111616884136*^9, 3.676111619101164*^9}, {
   3.67611165132108*^9, 3.67611182175666*^9}, 3.6761124388093357`*^9, {
   3.6761125310577183`*^9, 3.676112535417371*^9}, {3.67611278534865*^9, 
   3.676112852293083*^9}, {3.6761129469420633`*^9, 3.6761129566090517`*^9}, {
   3.6761133261675377`*^9, 3.6761133851728067`*^9}, {3.6761134570078707`*^9, 
   3.6761134886549597`*^9}, {3.676113519585043*^9, 3.676113581404831*^9}, {
   3.676113994051722*^9, 3.6761140495138607`*^9}, {3.67611471759483*^9, 
   3.676114727326268*^9}, {3.676114788083909*^9, 3.676114788267289*^9}, {
   3.67611493328951*^9, 3.6761149644016867`*^9}, {3.6761149993051443`*^9, 
   3.676115038096011*^9}, {3.676115419194528*^9, 3.676115484935451*^9}, {
   3.6761155900865583`*^9, 3.676115605985518*^9}, {3.676115760093829*^9, 
   3.676115778909306*^9}, {3.676115982499289*^9, 3.676116037133542*^9}, {
   3.6761161990700502`*^9, 3.676116233908482*^9}, {3.676116286507206*^9, 
   3.676116286712512*^9}, {3.67611639463448*^9, 3.676116464258135*^9}, {
   3.6761165873897543`*^9, 3.676116602580727*^9}, {3.6761174556753473`*^9, 
   3.67611745672952*^9}, {3.676117506535368*^9, 3.6761175229462357`*^9}, {
   3.676117600215383*^9, 3.6761176442364273`*^9}, {3.676117677849012*^9, 
   3.6761177082717953`*^9}, {3.67611812248459*^9, 3.6761181278825502`*^9}, {
   3.676118580166224*^9, 3.67611860485815*^9}, {3.676118651876894*^9, 
   3.6761187537386017`*^9}, {3.676118838160269*^9, 3.676118838374693*^9}, {
   3.67611887610327*^9, 3.676118940213615*^9}, {3.6761191096213007`*^9, 
   3.676119140757559*^9}, {3.6761192665343122`*^9, 3.676119273783136*^9}, {
   3.67611936142714*^9, 3.676119363325075*^9}, {3.676119488561606*^9, 
   3.676119492487294*^9}, {3.6761200703687267`*^9, 3.6761200707356052`*^9}, {
   3.6761202104386873`*^9, 3.676120268289008*^9}, {3.676120410677075*^9, 
   3.676120443293746*^9}, {3.676120474583932*^9, 3.676120475526285*^9}, {
   3.6761206125852633`*^9, 3.6761206205196333`*^9}, {3.6761234586125507`*^9, 
   3.676123489461384*^9}, {3.6761235483793917`*^9, 3.676123569634534*^9}, {
   3.676124493441423*^9, 3.676124518157651*^9}, {3.676124553443336*^9, 
   3.676124582966502*^9}, {3.6761246820730133`*^9, 3.676124716437358*^9}, {
   3.67612487549886*^9, 3.676124882235661*^9}, {3.676124956893395*^9, 
   3.676124958216114*^9}, {3.6761249904362507`*^9, 3.676124994364197*^9}, {
   3.676125048497746*^9, 3.676125189073619*^9}, {3.676125219923954*^9, 
   3.6761253767254953`*^9}, {3.6761261361020412`*^9, 3.676126180304117*^9}, {
   3.6761262318219957`*^9, 3.676126312208794*^9}, {3.6761263690337877`*^9, 
   3.6761264446350737`*^9}, {3.676126475578731*^9, 3.676126485467072*^9}, {
   3.676126538828438*^9, 3.676126562090301*^9}, {3.676126633785634*^9, 
   3.6761266365895157`*^9}, 3.6761268247458773`*^9, 3.676126912241247*^9, {
   3.676126960168599*^9, 3.676126960170166*^9}, {3.676127961375718*^9, 
   3.676127975299335*^9}, {3.676128268770321*^9, 3.676128269338847*^9}, 
   3.676128811729394*^9, 3.676723618051702*^9, {3.6767236778209963`*^9, 
   3.676723719771995*^9}, {3.676723979458334*^9, 3.676724090604282*^9}, {
   3.676724127937508*^9, 3.6767241297457438`*^9}, {3.676724160436442*^9, 
   3.676724221011488*^9}, {3.676724251830257*^9, 3.67672438224609*^9}, {
   3.676724439849156*^9, 3.676724475037467*^9}, 3.6767483498529*^9, 
   3.6780964291715813`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"QuinticTimeScaling", "[", 
      RowBox[{"Tf_", ",", "t_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Tf"}], ":", 
     RowBox[{
     "Total", " ", "time", " ", "of", " ", "the", " ", "motion", " ", "in", 
      " ", "seconds", " ", "from", " ", "rest", " ", "to", " ", "rest"}]}], 
    ",", 
    RowBox[{
     RowBox[{"t", ":", 
      RowBox[{
       RowBox[{
       "The", " ", "current", " ", "time", " ", "t", " ", "satisfying", " ", 
        "0"}], "<", "t", "<", 
       RowBox[{
        RowBox[{
        "Tf", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}], 
        " ", "s"}]}], ":", 
      RowBox[{
       RowBox[{"The", " ", "path", " ", "parameter", " ", "s", 
        RowBox[{"(", "t", ")"}], " ", "corresponding", " ", "to", " ", "a", 
        " ", "fifth"}], "-", 
       RowBox[{
       "order", " ", "polynomial", " ", "motion", " ", "that", " ", "begins", 
        " ", "and", "\n", "ends", " ", "at", " ", "zero", " ", "velocity", 
        " ", "and", " ", "zero", " ", 
        RowBox[{"acceleration", "."}]}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Tf"}], "=", "2"}], ";", "\n", 
     RowBox[{"t", "=", "0.6"}], ";", "\[IndentingNewLine]", 
     RowBox[{"s", " ", "=", " ", 
      RowBox[{
       RowBox[{"QuinticTimeScaling", "[", 
        RowBox[{"Tf", ",", "t"}], "]"}], "\[IndentingNewLine]", 
       "\[IndentingNewLine]", 
       RowBox[{
       "Output", ":", "\[IndentingNewLine]", "0.16308000000000003"}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"QuinticTimeScaling", "[", 
     RowBox[{"Tf_", ",", "t_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"s", ",", "a3", ",", "a4", ",", "a5"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{"(*", 
       RowBox[{"a0", ",", " ", "a1", ",", " ", 
        RowBox[{"and", " ", "a2", " ", "are", " ", "zero"}]}], "*)"}], 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"a3", "=", 
        RowBox[{"(", 
         RowBox[{"10", "/", 
          RowBox[{"(", 
           RowBox[{"Tf", "^", "3"}], ")"}]}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"a4", "=", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", "15"}], "/", 
          RowBox[{"(", 
           RowBox[{"Tf", "^", "4"}], ")"}]}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"a5", "=", " ", 
        RowBox[{"(", 
         RowBox[{"6", "/", 
          RowBox[{"(", 
           RowBox[{"Tf", "^", "5"}], ")"}]}], ")"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"s", "=", 
        RowBox[{
         RowBox[{"a3", "*", 
          RowBox[{"t", "^", "3"}]}], "+", 
         RowBox[{"a4", "*", 
          RowBox[{"t", "^", "4"}]}], "+", 
         RowBox[{"a5", "*", 
          RowBox[{"t", "^", "5"}]}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "s", "]"}], ";"}]}], "]"}]}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.6767251794253798`*^9, 3.676725236664359*^9}, {
   3.676725277944274*^9, 3.67672530444382*^9}, {3.676725421409651*^9, 
   3.6767254615354853`*^9}, {3.676725541834879*^9, 3.67672557969561*^9}, {
   3.676725634863492*^9, 3.67672567485637*^9}, {3.676725730380808*^9, 
   3.676725775670374*^9}, 3.6780964362059727`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"JointTrajectory", "[", 
      RowBox[{
      "thetastart_", ",", "thetaend_", ",", "Tf_", ",", "N_", ",", 
       "method_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetastart"}], ":", 
     RowBox[{"The", " ", "initial", " ", "joint", " ", "variables"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"thetaend", ":", 
     RowBox[{"The", " ", "final", " ", "joint", " ", "variables"}]}], ",", 
    RowBox[{"Tf", ":", 
     RowBox[{
     "Total", " ", "time", " ", "of", " ", "the", " ", "motion", " ", "in", 
      " ", "seconds", " ", "from", " ", "rest", " ", "to", " ", "rest"}]}], 
    ",", 
    RowBox[{"N", ":", 
     RowBox[{
      RowBox[{"The", " ", "number", " ", "of", " ", "points", " ", "N"}], ">", 
      RowBox[{"1", " ", 
       RowBox[{"(", 
        RowBox[{"Start", " ", "and", " ", "stop"}], ")"}], " ", "in", " ", 
       "the", " ", "discrete", " ", "representation", " ", "of", " ", "the", 
       " ", "trajectory"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"method", ":", 
     RowBox[{
      RowBox[{"The", " ", "time"}], "-", 
      RowBox[{"scaling", " ", "method"}]}]}], ",", 
    RowBox[{"where", " ", "3", " ", "indicates", " ", "cubic", " ", 
     RowBox[{"(", 
      RowBox[{"third", "-", 
       RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
     "scaling", "\n", "and", " ", "5", " ", "indicates", " ", "quintic", " ", 
     
     RowBox[{"(", 
      RowBox[{"fifth", "-", 
       RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
     RowBox[{
     "scaling", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
      "Returns"}], " ", 
     RowBox[{"traj", ":", 
      RowBox[{
      "A", " ", "trajectory", " ", "as", " ", "an", " ", "N", " ", "x", " ", 
       "n", " ", "matrix"}]}]}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
        "where", " ", "each", " ", "row", " ", "is", " ", "an", " ", "n"}], 
        "-", 
        RowBox[{
        "vector", " ", "of", " ", "joint", " ", "variables", " ", "at", " ", 
         "an", "\n", "instant", " ", "in", " ", 
         RowBox[{"time", ".", "The"}], " ", "first", " ", "row", " ", "is", 
         " ", "thetastart", " ", "and", " ", "the", " ", "Nth", " ", "row", 
         " ", "is", " ", 
         RowBox[{"thetaend", ".", "The"}], " ", "elapsed", " ", "time", " ", 
         "between", " ", "each", " ", "row", " ", "is", " ", 
         RowBox[{"Tf", "/", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"N", "-", "1"}], ")"}], ".", "\[IndentingNewLine]", 
           "The"}]}], " ", "returned", " ", "trajectory", " ", "is", " ", "a",
          " ", "straight"}], "-", 
        RowBox[{"line", " ", "motion", " ", "in", " ", "joint", " ", 
         RowBox[{"space", ".", "\[IndentingNewLine]", "Animation"}], " ", 
         "example", " ", "can", " ", "be", " ", "seen", " ", "at", " ", 
         RowBox[{"https", ":"}]}]}], "//", 
       RowBox[{
        RowBox[{"www", ".", "youtube", ".", "com"}], "/", 
        RowBox[{"watch", "?", "v"}]}]}], "=", "fVElSuS1GgI"}], 
     "\[IndentingNewLine]", ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "thetastart"}], "=", 
      RowBox[{"{", 
       RowBox[{
       "1", ",", "0", ",", "0", ",", "1", ",", "1", ",", "0.2", ",", "0", ",",
         "1"}], "}"}]}], ";", "\n", 
     RowBox[{"thetaend", "=", 
      RowBox[{"{", 
       RowBox[{
       "1.2", ",", "0.5", ",", "0.6", ",", "1.1", ",", "2", ",", "2", ",", 
        "0.9", ",", "1"}], "}"}]}], ";", "\n", 
     RowBox[{"Tf", "=", "4"}], ";", "\n", 
     RowBox[{"Np", "=", "6"}], ";", "\n", 
     RowBox[{"method", "=", "3"}], ";", "\[IndentingNewLine]", 
     RowBox[{"traj", " ", "=", " ", 
      RowBox[{
       RowBox[{
        RowBox[{"JointTrajectory", "[", 
         RowBox[{
         "thetastart", ",", "thetaend", ",", "Tf", ",", "Np", ",", "method"}],
          "]"}], "//", "MatrixForm"}], "//", 
       RowBox[{"N", "\[IndentingNewLine]", "\[IndentingNewLine]", 
        RowBox[{"Output", ":", "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1.", "0.", "0.", "1.", "1.", "0.2", "0.", "1."},
             {"1.0208", "0.052", "0.0624", "1.0104", "1.104", "0.3872", 
              "0.0936", "1."},
             {"1.0704", "0.176", "0.2112", "1.0352", "1.352", "0.8336", 
              "0.3168", "1."},
             {"1.1296", "0.324", "0.3888", "1.0648", "1.648", "1.3664", 
              "0.5832", "1."},
             {"1.1792", "0.448", "0.5376", "1.0896", "1.896", "1.8128", 
              "0.8064", "1."},
             {"1.2", "0.5", "0.6", "1.1", "2.", "2.", "0.9", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"JointTrajectory", "[", 
     RowBox[{
     "thetastart_", ",", "thetaend_", ",", "Tf_", ",", "N_", ",", "method_"}],
      "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"traj", ",", "timegap", ",", "s", ",", "thetastep"}], "}"}], 
      ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"timegap", " ", "=", " ", 
        RowBox[{"Tf", "/", 
         RowBox[{"(", 
          RowBox[{"N", "-", "1"}], ")"}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"traj", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"For", "[", 
        RowBox[{
         RowBox[{"i", " ", "=", " ", "0"}], ",", 
         RowBox[{"i", "\[LessEqual]", " ", 
          RowBox[{"N", "-", "1"}]}], ",", 
         RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"Which", "[", 
           RowBox[{
            RowBox[{"method", " ", "\[Equal]", " ", "3"}], ",", 
            RowBox[{"s", "=", " ", 
             RowBox[{"CubicTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}], ",", 
            RowBox[{"method", " ", "\[Equal]", " ", "5"}], ",", 
            RowBox[{"s", " ", "=", " ", 
             RowBox[{"QuinticTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}]}], "]"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"thetastep", " ", "=", " ", 
           RowBox[{"thetastart", "  ", "+", " ", 
            RowBox[{"s", 
             RowBox[{"(", 
              RowBox[{"thetaend", " ", "-", " ", "thetastart"}], ")"}]}]}]}], 
          ";", "\[IndentingNewLine]", 
          RowBox[{"traj", " ", "=", " ", 
           RowBox[{"Insert", "[", 
            RowBox[{"traj", ",", "thetastep", ",", 
             RowBox[{"i", "+", "1"}]}], "]"}]}], ";"}]}], 
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "traj", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.67672532250014*^9, 
   3.676725326694919*^9}, {3.676726238098218*^9, 3.6767262868714447`*^9}, {
   3.67672632351712*^9, 3.676726328872868*^9}, {3.6767269746668167`*^9, 
   3.676726982754202*^9}, {3.676727016189947*^9, 3.6767270198275414`*^9}, {
   3.676727056006617*^9, 3.676727061995698*^9}, {3.676727162429737*^9, 
   3.676727252326495*^9}, {3.676727300513076*^9, 3.6767274904689007`*^9}, {
   3.6767382395189667`*^9, 3.676738245456195*^9}, 3.676743247626157*^9, {
   3.676748358463169*^9, 3.6767483602563457`*^9}, 3.678096441208591*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"ScrewTrajectory", "[", 
      RowBox[{
      "Xstart_", ",", "Xend_", ",", "Tf_", ",", "N_", ",", "method_"}], "]"}],
      ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Xstart"}], ":", 
     RowBox[{
      RowBox[{"The", " ", "initial", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration"}]}]}], ",", 
    RowBox[{"Xend", ":", 
     RowBox[{
      RowBox[{"The", " ", "final", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration"}]}]}], ",", 
    RowBox[{"Tf", ":", 
     RowBox[{
     "Total", " ", "time", " ", "of", " ", "the", " ", "motion", " ", "in", 
      " ", "seconds", " ", "from", " ", "rest", " ", "to", " ", "rest"}]}], 
    ",", 
    RowBox[{"N", ":", 
     RowBox[{
      RowBox[{"The", " ", "number", " ", "of", " ", "points", " ", "N"}], ">", 
      RowBox[{"1", " ", 
       RowBox[{"(", 
        RowBox[{"Start", " ", "and", " ", "stop"}], ")"}], " ", "in", " ", 
       "the", " ", "discrete", " ", "representation", " ", "of", " ", "the", 
       " ", "trajectory"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"method", ":", 
     RowBox[{
      RowBox[{"The", " ", "time"}], "-", 
      RowBox[{"scaling", " ", "method"}]}]}], ",", " ", 
    RowBox[{
     RowBox[{"where", " ", "3", " ", "indicates", " ", "cubic", " ", 
      RowBox[{"(", 
       RowBox[{"third", "-", 
        RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
      "scaling", "\n", "and", " ", "5", " ", "indicates", " ", "quintic", " ", 
      RowBox[{"(", 
       RowBox[{"fifth", "-", 
        RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
      RowBox[{
      "scaling", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
       "Returns"}], " ", 
      RowBox[{"traj", ":", 
       RowBox[{
       "The", " ", "discretized", " ", "trajectory", " ", "as", " ", "a", " ",
         "list", " ", "of", " ", "N", " ", "matrices", " ", "in", " ", "SE", 
        RowBox[{"(", "3", ")"}], " ", "separated", " ", "in", "\n", "time", 
        " ", "by", " ", 
        RowBox[{"Tf", "/", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"N", "-", "1"}], ")"}], ".", "The"}]}], " ", "first", " ", 
        "in", " ", "the", " ", "list", " ", "is", " ", "Xstart", " ", "and", 
        " ", "the", " ", "Nth", " ", "is", " ", 
        RowBox[{"Xend", ".", "\[IndentingNewLine]", "This"}], " ", "function",
         " ", "calculates", " ", "a", " ", "trajectory", " ", "corresponding",
         " ", "to", " ", "the", " ", "screw", " ", "motion", " ", "about", 
        " ", "a", " ", "space", " ", "screw", " ", 
        RowBox[{"axis", "."}]}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Xstart"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Xend", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "0.1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "4.1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Tf", "=", "5"}], ";", "\n", 
     RowBox[{"Np", "=", "4"}], ";", "\n", 
     RowBox[{"method", "=", "3"}], ";", "\[IndentingNewLine]", 
     "\[IndentingNewLine]", 
     RowBox[{"traj", " ", "=", " ", 
      RowBox[{"ScrewTrajectory", "[", 
       RowBox[{"Xstart", ",", "Xend", ",", "Tf", ",", "Np", ",", "method"}], 
       "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{"traj", "[", 
          RowBox[{"[", "1", "]"}], "]"}], "//", 
         RowBox[{"MatrixForm", "\[IndentingNewLine]", 
          RowBox[{"traj", "[", 
           RowBox[{"[", "2", "]"}], "]"}]}]}], "//", 
        RowBox[{"MatrixForm", "\[IndentingNewLine]", 
         RowBox[{"traj", "[", 
          RowBox[{"[", "3", "]"}], "]"}]}]}], "//", 
       RowBox[{"MatrixForm", "\[IndentingNewLine]", 
        RowBox[{"traj", "[", 
         RowBox[{"[", "4", "]"}], "]"}]}]}], "//", 
      RowBox[{"MatrixForm", "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1.", "0.", "0.", "1."},
             {"0.", "1.", "0.", "0."},
             {"0.", "0.", "1.", "1."},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.904", 
              RowBox[{"-", "0.25"}], "0.346", "0.441"},
             {"0.346", "0.904", 
              RowBox[{"-", "0.25"}], "0.529"},
             {
              RowBox[{"-", "0.25"}], "0.346", "0.904", "1.601"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.346", 
              RowBox[{"-", "0.25"}], "0.904", 
              RowBox[{"-", "0.117"}]},
             {"0.904", "0.346", 
              RowBox[{"-", "0.25"}], "0.473"},
             {
              RowBox[{"-", "0.25"}], "0.904", "0.346", "3.274"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\[IndentingNewLine]", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.", "0.", "1.", "0.1"},
             {"1.", "0.", "0.", "0."},
             {"0.", "1.", "0.", "4.1"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"ScrewTrajectory", "[", 
     RowBox[{"Xstart_", ",", "Xend_", ",", "Tf_", ",", "N_", ",", "method_"}],
      "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"traj", ",", "timegap", ",", "s", ",", "Xstep"}], "}"}], ",", 
      "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"timegap", " ", "=", " ", 
        RowBox[{"Tf", "/", 
         RowBox[{"(", 
          RowBox[{"N", "-", "1"}], ")"}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"traj", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"For", "[", 
        RowBox[{
         RowBox[{"i", " ", "=", " ", "0"}], ",", 
         RowBox[{"i", "\[LessEqual]", " ", 
          RowBox[{"N", "-", "1"}]}], ",", 
         RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"Which", "[", 
           RowBox[{
            RowBox[{"method", " ", "\[Equal]", " ", "3"}], ",", 
            RowBox[{"s", "=", " ", 
             RowBox[{"CubicTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}], ",", 
            RowBox[{"method", " ", "\[Equal]", " ", "5"}], ",", 
            RowBox[{"s", " ", "=", " ", 
             RowBox[{"QuinticTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}]}], "]"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Xstep", " ", "=", " ", 
           RowBox[{"Xstart", ".", 
            RowBox[{"MatrixExp6", "[", 
             RowBox[{
              RowBox[{"MatrixLog6", "[", 
               RowBox[{
                RowBox[{"TransInv", "[", "Xstart", "]"}], ".", "Xend"}], 
               "]"}], "*", "s"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
          RowBox[{"traj", " ", "=", " ", 
           RowBox[{"Insert", "[", 
            RowBox[{"traj", ",", "Xstep", ",", 
             RowBox[{"i", "+", "1"}]}], "]"}]}], ";"}]}], 
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "traj", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.6767384630832443`*^9, 
   3.6767384852301083`*^9}, {3.676738544986739*^9, 3.676738607109634*^9}, {
   3.676739097041395*^9, 3.676739116485881*^9}, {3.6767393261296473`*^9, 
   3.676739361702149*^9}, {3.6767396930954237`*^9, 3.676739694812849*^9}, 
   3.676739769624935*^9, {3.676739804626966*^9, 3.676739852892261*^9}, {
   3.676741390075983*^9, 3.676741723511598*^9}, 3.6774898932912083`*^9, 
   3.678096448832265*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"CartesianTrajectory", "[", 
      RowBox[{
      "Xstart_", ",", "Xend_", ",", "Tf_", ",", "N_", ",", "method_"}], "]"}],
      ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "Xstart"}], ":", 
     RowBox[{
      RowBox[{"The", " ", "initial", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration"}]}]}], ",", 
    RowBox[{"Xend", ":", 
     RowBox[{
      RowBox[{"The", " ", "final", " ", "end"}], "-", 
      RowBox[{"effector", " ", "configuration"}]}]}], ",", 
    RowBox[{"Tf", ":", 
     RowBox[{
     "Total", " ", "time", " ", "of", " ", "the", " ", "motion", " ", "in", 
      " ", "seconds", " ", "from", " ", "rest", " ", "to", " ", "rest"}]}], 
    ",", 
    RowBox[{"N", ":", 
     RowBox[{
      RowBox[{"The", " ", "number", " ", "of", " ", "points", " ", "N"}], ">", 
      RowBox[{"1", " ", 
       RowBox[{"(", 
        RowBox[{"Start", " ", "and", " ", "stop"}], ")"}], " ", "in", " ", 
       "the", " ", "discrete", " ", "representation", " ", "of", " ", "the", 
       " ", "trajectory"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"method", ":", 
     RowBox[{
      RowBox[{"The", " ", "time"}], "-", 
      RowBox[{"scaling", " ", "method"}]}]}], ",", 
    RowBox[{"where", " ", "3", " ", "indicates", " ", "cubic", " ", 
     RowBox[{"(", 
      RowBox[{"third", "-", 
       RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
     "scaling", " ", "and", " ", "5", " ", "indicates", " ", "quintic", " ", 
     RowBox[{"(", 
      RowBox[{"fifth", "-", 
       RowBox[{"order", " ", "polynomial"}]}], ")"}], " ", "time", " ", 
     RowBox[{
     "scaling", ".", "\[IndentingNewLine]", "\[IndentingNewLine]", 
      "Returns"}], " ", 
     RowBox[{"traj", ":", " ", 
      RowBox[{
      "The", " ", "discretized", " ", "trajectory", " ", "as", " ", "a", " ", 
       "list", " ", "of", " ", "N", " ", "matrices", " ", "in", " ", "SE", 
       RowBox[{"(", "3", ")"}], " ", "separated", " ", "in", " ", "time", " ",
        "by", " ", 
       RowBox[{"Tf", "/", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{"N", "-", "1"}], ")"}], ".", "The"}]}], " ", "first", " ", 
       "in", " ", "the", " ", "list", " ", "is", " ", "Xstart", " ", "and", 
       " ", "the", " ", "Nth", " ", "is", " ", 
       RowBox[{"Xend", ".", " ", "This"}], " ", "function", " ", "is", " ", 
       "Similar", " ", "to", " ", "ScrewTrajectory"}]}]}], ",", 
    RowBox[{
     RowBox[{
     "except", " ", "the", " ", "origin", " ", "of", " ", "the", " ", "end"}],
      "-", 
     RowBox[{
     "effector", " ", "frame", " ", "follows", " ", "a", " ", "straight", " ",
       "line"}]}], ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{"decoupled", " ", "from", " ", "the", " ", "rotational", " ", 
        RowBox[{"motion", ".", "\[IndentingNewLine]", "Animation"}], " ", 
        "example", " ", "can", " ", "be", " ", "seen", " ", "at", " ", 
        RowBox[{"https", ":"}]}], "//", 
       RowBox[{
        RowBox[{"www", ".", "youtube", ".", "com"}], "/", 
        RowBox[{"watch", "?", "v"}]}]}], "=", 
      RowBox[{"ycaGRk_", "0", "AE8"}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Example", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "Xstart"}], "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Xend", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "1", ",", "0.1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0", ",", "4.1"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Tf", "=", "5"}], ";", "\n", 
     RowBox[{"Np", "=", "4"}], ";", "\n", 
     RowBox[{"method", "=", "5"}], ";", "\[IndentingNewLine]", 
     "\[IndentingNewLine]", 
     RowBox[{"traj", " ", "=", " ", 
      RowBox[{"CartesianTrajectory", "[", 
       RowBox[{"Xstart", ",", "Xend", ",", "Tf", ",", "Np", ",", "method"}], 
       "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{
            RowBox[{
             RowBox[{"traj", "[", 
              RowBox[{"[", "1", "]"}], "]"}], "//", "MatrixForm"}], "//", 
            RowBox[{"N", "\[IndentingNewLine]", 
             RowBox[{"traj", "[", 
              RowBox[{"[", "2", "]"}], "]"}]}]}], "//", "MatrixForm"}], "//", 
          
          RowBox[{"N", "\[IndentingNewLine]", 
           RowBox[{"traj", "[", 
            RowBox[{"[", "3", "]"}], "]"}]}]}], "//", "MatrixForm"}], "//", 
        RowBox[{"N", "\[IndentingNewLine]", 
         RowBox[{"traj", "[", 
          RowBox[{"[", "4", "]"}], "]"}]}]}], "//", "MatrixForm"}], "//", 
      RowBox[{"N", "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Output", ":", "\[IndentingNewLine]", 
        RowBox[{
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"1.", "0.", "0.", "1."},
             {"0.", "1.", "0.", "0."},
             {"0.", "0.", "1.", "1."},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\n", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.937", 
              RowBox[{"-", "0.214"}], "0.277", "0.811"},
             {"0.277", "0.937", 
              RowBox[{"-", "0.214"}], "0."},
             {
              RowBox[{"-", "0.214"}], "0.277", "0.937", "1.651"},
             {"0.`", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\n", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.277", 
              RowBox[{"-", "0.214"}], "0.937", "0.289"},
             {"0.937", "0.277", 
              RowBox[{"-", "0.214"}], "0."},
             {
              RowBox[{"-", "0.214"}], "0.937", "0.277", "3.449"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]], "\n", 
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"0.", "0.", "1.", "0.1"},
             {"1.", "0.", "0.", "0."},
             {"0.", "1.", "0.", "4.1"},
             {"0.", "0.", "0.", "1."}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]]}]}]}]}]}]}], "\[IndentingNewLine]", 
   "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"CartesianTrajectory", "[", 
     RowBox[{"Xstart_", ",", "Xend_", ",", "Tf_", ",", "N_", ",", "method_"}],
      "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "timegap", ",", "s", ",", "Rcurrent", ",", "Pcurrent", ",", "traj", 
        ",", "Xcurrent", ",", "Rstart", ",", "Rend", ",", "Pstart", ",", 
        "Pend"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"timegap", " ", "=", " ", 
        RowBox[{"Tf", "/", 
         RowBox[{"(", 
          RowBox[{"N", "-", "1"}], ")"}]}]}], ";", "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"Rstart", ",", "Pstart"}], "}"}], " ", "=", " ", 
        RowBox[{"TransToRp", "[", "Xstart", "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"Rend", ",", "Pend"}], "}"}], " ", "=", " ", 
        RowBox[{"TransToRp", "[", "Xend", "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"traj", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"For", "[", 
        RowBox[{
         RowBox[{"i", " ", "=", " ", "0"}], ",", 
         RowBox[{"i", "\[LessEqual]", " ", 
          RowBox[{"N", "-", "1"}]}], ",", 
         RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"Which", "[", 
           RowBox[{
            RowBox[{"method", " ", "\[Equal]", " ", "3"}], ",", 
            RowBox[{"s", "=", " ", 
             RowBox[{"CubicTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}], ",", 
            RowBox[{"method", " ", "\[Equal]", " ", "5"}], ",", 
            RowBox[{"s", " ", "=", " ", 
             RowBox[{"QuinticTimeScaling", "[", 
              RowBox[{"Tf", ",", 
               RowBox[{"i", "*", "timegap"}]}], "]"}]}]}], "]"}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Rcurrent", " ", "=", " ", 
           RowBox[{"Rstart", ".", 
            RowBox[{"MatrixExp", "[", 
             RowBox[{"VecToso3", "[", 
              RowBox[{
               RowBox[{"MatrixLog3", "[", 
                RowBox[{
                 RowBox[{"RotInv", "[", "Rstart", "]"}], ".", "Rend"}], "]"}],
                "*", "s"}], "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
          RowBox[{"Pcurrent", " ", "=", " ", 
           RowBox[{"Pstart", " ", "+", " ", 
            RowBox[{"s", 
             RowBox[{"(", 
              RowBox[{"Pend", " ", "-", " ", "Pstart"}], ")"}]}]}]}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"Xcurrent", " ", "=", " ", 
           RowBox[{"RpToTrans", "[", 
            RowBox[{"Rcurrent", ",", "Pcurrent"}], "]"}]}], ";", 
          "\[IndentingNewLine]", 
          RowBox[{"traj", " ", "=", " ", 
           RowBox[{"Insert", "[", 
            RowBox[{"traj", ",", "Xcurrent", ",", 
             RowBox[{"i", "+", "1"}]}], "]"}]}], ";"}]}], 
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "traj", "]"}], ";"}]}], "\[IndentingNewLine]", 
     "]"}]}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.67673963558117*^9, 
   3.6767396395269127`*^9}, {3.676743278578334*^9, 3.676743282140527*^9}, {
   3.676743329578144*^9, 3.676743332931324*^9}, {3.676743375759927*^9, 
   3.676743393331377*^9}, {3.676743579279604*^9, 3.676743610992496*^9}, {
   3.676743696815421*^9, 3.676743702063456*^9}, {3.676743797327611*^9, 
   3.676743802367258*^9}, {3.676743844436429*^9, 3.676743923034832*^9}, {
   3.676744067943886*^9, 3.676744101485681*^9}, {3.676745202969133*^9, 
   3.676745215144767*^9}, {3.676745254062787*^9, 3.676745307013712*^9}, {
   3.6767453632225313`*^9, 3.676745371103043*^9}, 3.676745427635807*^9, 
   3.676745643761804*^9, {3.6767456813682833`*^9, 3.676745685052404*^9}, {
   3.676745890112192*^9, 3.676745904905205*^9}, {3.676745949389865*^9, 
   3.676746003820777*^9}, {3.676746191330016*^9, 3.676746202180275*^9}, {
   3.6767463628997517`*^9, 3.6767463680370293`*^9}, {3.6767464041150723`*^9, 
   3.6767464065122213`*^9}, {3.676746447029583*^9, 3.676746538635809*^9}, {
   3.67674660560835*^9, 3.676746624390595*^9}, {3.676746756856359*^9, 
   3.676746788930705*^9}, {3.67674687514357*^9, 3.6767469138687143`*^9}, {
   3.676746954576331*^9, 3.67674698750915*^9}, 3.676747020885099*^9, {
   3.6767472587172203`*^9, 3.676747387781352*^9}, {3.676748382220996*^9, 
   3.676748383415188*^9}, 3.678096454219871*^9}]
}, Closed]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["CHAPTER  11: ROBOT CONTROL",
 FontSize->24,
 FontWeight->"Bold",
 FontColor->RGBColor[0, 0, 1]]], "Title",
 CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
  3.675531958735077*^9, 3.67553201284205*^9}, {3.675607857109823*^9, 
  3.6756079117244864`*^9}, {3.6756821024917507`*^9, 3.6756821283850946`*^9}, {
  3.6757648758196497`*^9, 3.675764892595064*^9}, {3.677861669528915*^9, 
  3.677861687077693*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"ComputedTorque", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "eint_", ",", "g_", ",", 
       "Mlist_", ",", "Glist_", ",", "Slist_", ",", "thetalistd_", ",", 
       "dthetalistd_", ",", "ddthetalistd_", ",", "Kp_", ",", "Ki_", ",", 
       "Kd_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", " ", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "variables"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "joint", " ", "rates"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"eint", ":", 
     RowBox[{"n", "-", 
      RowBox[{"vector", " ", "of", " ", "the", " ", "time"}], "-", 
      RowBox[{"integral", " ", "of", " ", "joint", " ", "errors"}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"Gravity", " ", "vector", " ", "g"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Mlist", ":", 
     RowBox[{"List", " ", "of", " ", "link", " ", "frames", " ", 
      RowBox[{"{", "i", "}"}], " ", "relative", " ", "to", " ", 
      RowBox[{"{", 
       RowBox[{"i", "-", "1"}], "}"}], " ", "at", " ", "the", " ", "home", 
      " ", "position"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Spatial", " ", "inertia", " ", "matrices", " ", "Gi", " ", "of", " ", 
      "the", " ", "links"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
      " ", "in", " ", "a", " ", "space", " ", "frame"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"thetalistd", ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "reference", " ", "joint", " ", 
       "variables"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"dthetalistd", ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "reference", " ", "joint", " ", 
       "velocities"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"ddthetalistd", ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "reference", " ", "joint", " ", 
       "accelerations"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Kp", ":", 
     RowBox[{"The", " ", "feedback", " ", "proportional", " ", "gain", " ", 
      RowBox[{"(", 
       RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], 
       ")"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Ki", ":", 
     RowBox[{"The", " ", "feedback", " ", "integral", " ", "gain", " ", 
      RowBox[{"(", 
       RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], 
       ")"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"Kd", ":", 
      RowBox[{"The", " ", "feedback", " ", "derivative", " ", "gain", " ", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], ")"}], 
        ".", "\[IndentingNewLine]", "\[IndentingNewLine]", "Returns"}], " ", 
       "taulist"}], ":", 
      RowBox[{"The", " ", "vector", " ", "of", " ", "joint", " ", 
       RowBox[{"forces", "/", "torques"}], " ", "computed", " ", "by", " ", 
       "the", " ", "feedback", " ", "linearizing", " ", "controller", " ", 
       "at", " ", "the", " ", "current", " ", 
       RowBox[{"instant", "."}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Exapmle", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", 
     RowBox[{"eint", "=", 
      RowBox[{"{", 
       RowBox[{"0.2", ",", "0.2", ",", "0.2"}], "}"}]}], ";", "\n", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "0.23", ",", "0.1"}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "1.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0.2"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"thetalistd", "=", 
      RowBox[{"{", 
       RowBox[{"1", ",", "1", ",", "1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalistd", "=", 
      RowBox[{"{", 
       RowBox[{"2", ",", "1.2", ",", "2"}], "}"}]}], ";", "\n", 
     RowBox[{"ddthetalistd", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"Kp", "=", "1.3"}], ";", "\n", 
     RowBox[{"Ki", "=", "1.2"}], ";", "\n", 
     RowBox[{"Kd", "=", "1.1"}], ";", "\[IndentingNewLine]", 
     RowBox[{"taulist", " ", "=", " ", 
      RowBox[{
       RowBox[{"ComputedTorque", "[", 
        RowBox[{
        "thetalist", ",", "dthetalist", ",", "eint", ",", "g", ",", "Mlist", 
         ",", "Glist", ",", "Slist", ",", "thetalistd", ",", "dthetalistd", 
         ",", "ddthetalistd", ",", "Kp", ",", "Ki", ",", "Kd"}], "]"}], 
       "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Ouput", ":", "\[IndentingNewLine]", 
        RowBox[{"{", 
         RowBox[{"7.4417545773188", ",", "1.3249366065459927", ",", 
          RowBox[{"-", "4.818081471780161"}]}], "}"}]}]}]}]}]}], 
   "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"ComputedTorque", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "eint_", ",", "g_", ",", "Mlist_",
       ",", "Glist_", ",", "Slist_", ",", "thetalistd_", ",", "dthetalistd_", 
      ",", "ddthetalistd_", ",", "Kp_", ",", "Ki_", ",", "Kd_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "taulist", ",", " ", "e", ",", " ", "eder", ",", "M", ",", " ", "c", 
        ",", " ", "grav"}], "}"}], ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"e", "=", 
        RowBox[{"thetalistd", "-", "thetalist"}]}], ";", "\n", 
       RowBox[{"eder", "=", 
        RowBox[{"dthetalistd", "-", "dthetalist"}]}], ";", 
       "\[IndentingNewLine]", "\n", 
       RowBox[{"M", "=", 
        RowBox[{"MassMatrix", "[", 
         RowBox[{"thetalist", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
         "]"}]}], ";", "\n", 
       RowBox[{"c", "=", 
        RowBox[{"VelQuadraticForces", "[", 
         RowBox[{
         "thetalist", ",", "dthetalist", ",", "Mlist", ",", "Glist", ",", 
          "Slist"}], "]"}]}], ";", "\n", 
       RowBox[{"grav", "=", 
        RowBox[{"GravityForces", "[", 
         RowBox[{
         "thetalist", ",", "g", ",", "Mlist", ",", "Glist", ",", "Slist"}], 
         "]"}]}], ";", "\[IndentingNewLine]", "\n", 
       RowBox[{"taulist", "=", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{"M", ".", 
           RowBox[{"(", 
            RowBox[{"ddthetalistd", "+", 
             RowBox[{"(", 
              RowBox[{"Kp", "*", "e"}], ")"}], "+", 
             RowBox[{"(", 
              RowBox[{"Ki", "*", "eint"}], ")"}], "+", 
             RowBox[{"(", 
              RowBox[{"Kd", "*", "eder"}], ")"}]}], ")"}]}], ")"}], "+", "c", 
         "+", "grav"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Return", "[", "taulist", "]"}], ";"}]}], 
     "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
   3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9, 
   3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
   3.6541080276373253`*^9, 3.654108039845397*^9}, {3.654108208328875*^9, 
   3.654108267778264*^9}, {3.654108316603101*^9, 3.65410832858707*^9}, {
   3.654122072990996*^9, 3.654122200142358*^9}, {3.654122232524707*^9, 
   3.6541222637488813`*^9}, {3.654122372486209*^9, 3.65412253592478*^9}, {
   3.654122571686759*^9, 3.654122685636675*^9}, {3.654122740902017*^9, 
   3.6541228215630703`*^9}, {3.654122865671712*^9, 3.654122935620612*^9}, {
   3.654123032181581*^9, 3.6541230914473333`*^9}, {3.654123152783122*^9, 
   3.6541232043612537`*^9}, {3.654123304611115*^9, 3.654123341073502*^9}, {
   3.654123502235703*^9, 3.654123578362132*^9}, {3.654123664279245*^9, 
   3.654123683195951*^9}, {3.6541237878998413`*^9, 3.6541238249788847`*^9}, {
   3.654123940392714*^9, 3.654123947510108*^9}, {3.654124026309593*^9, 
   3.654124026885428*^9}, 3.6541241152729683`*^9, 3.654124170343902*^9, {
   3.6541246995439568`*^9, 3.654124712871656*^9}, {3.6541251843297157`*^9, 
   3.654125231518578*^9}, {3.6541254856851683`*^9, 3.65412548908077*^9}, {
   3.654125547742125*^9, 3.654125551165526*^9}, {3.654125930807577*^9, 
   3.654126007244483*^9}, {3.654126127665163*^9, 3.654126191634789*^9}, {
   3.654126236482831*^9, 3.654126250153975*^9}, {3.6541262848658533`*^9, 
   3.654126288991514*^9}, {3.654126323055903*^9, 3.654126399729628*^9}, {
   3.654126435349374*^9, 3.654126447266036*^9}, {3.654126530077552*^9, 
   3.6541265937421*^9}, {3.654126679867402*^9, 3.6541267650289497`*^9}, {
   3.6541267969631968`*^9, 3.654126798410089*^9}, {3.654126829860013*^9, 
   3.654126881359337*^9}, {3.6541269393824577`*^9, 3.654126959132009*^9}, {
   3.654127079773924*^9, 3.654127194954756*^9}, {3.654127229244482*^9, 
   3.654127253953713*^9}, {3.6541278694900227`*^9, 3.654127878663176*^9}, {
   3.654127969775614*^9, 3.6541279938293123`*^9}, {3.654128051763153*^9, 
   3.654128134016169*^9}, {3.654128326830834*^9, 3.654128326972948*^9}, {
   3.654182305132535*^9, 3.654182317491791*^9}, {3.65418238746202*^9, 
   3.654182511491269*^9}, {3.654182551115921*^9, 3.654182559209998*^9}, {
   3.6541826265390177`*^9, 3.654182732425318*^9}, {3.654182795218339*^9, 
   3.654182802306821*^9}, {3.654182858360195*^9, 3.654182976637673*^9}, {
   3.65418309291822*^9, 3.6541831115452337`*^9}, {3.654183280620055*^9, 
   3.654183381285778*^9}, {3.6541834216733217`*^9, 3.65418343026462*^9}, {
   3.65418347398075*^9, 3.654183479016459*^9}, {3.654183517743698*^9, 
   3.654183590244978*^9}, {3.654183629340012*^9, 3.654183659551435*^9}, {
   3.654183828746057*^9, 3.654183869187777*^9}, {3.654183918271063*^9, 
   3.6541839184304667`*^9}, {3.654206461375032*^9, 3.6542064948942213`*^9}, {
   3.6542065261576433`*^9, 3.654206710642645*^9}, {3.65420685898112*^9, 
   3.654206859498214*^9}, {3.654206912388175*^9, 3.6542069199221067`*^9}, {
   3.654206956850091*^9, 3.654206957769821*^9}, {3.6542070885864773`*^9, 
   3.6542070905801163`*^9}, {3.6542071861274023`*^9, 3.654207197287834*^9}, {
   3.654207355631957*^9, 3.65420737546005*^9}, {3.654207422116619*^9, 
   3.6542074295958633`*^9}, {3.654207513008601*^9, 3.654207622631179*^9}, {
   3.6542077902321663`*^9, 3.6542079082040863`*^9}, {3.654208112675095*^9, 
   3.6542082406765347`*^9}, {3.654208390436673*^9, 3.654208437282127*^9}, {
   3.654208729398788*^9, 3.6542088242969923`*^9}, {3.654208875181876*^9, 
   3.6542089105814543`*^9}, {3.654209036563011*^9, 3.654209042056604*^9}, {
   3.654209131808134*^9, 3.654209211748107*^9}, {3.654277910558556*^9, 
   3.654277962902483*^9}, {3.6542785554956903`*^9, 3.654278555764879*^9}, {
   3.6542801407928543`*^9, 3.654280145647377*^9}, {3.654280336511973*^9, 
   3.654280388661951*^9}, {3.654735519742467*^9, 3.654735526212496*^9}, {
   3.654736711632904*^9, 3.654736744359725*^9}, {3.654736840726715*^9, 
   3.6547369027091503`*^9}, {3.6547369619155817`*^9, 3.65473711893006*^9}, 
   3.654737215212165*^9, {3.6547372623430433`*^9, 3.654737301717353*^9}, {
   3.654737434898181*^9, 3.654737445769989*^9}, {3.654737593349925*^9, 
   3.654737893647337*^9}, {3.654737938282483*^9, 3.654737939560112*^9}, {
   3.654737995469615*^9, 3.65473804382544*^9}, {3.65473814244207*^9, 
   3.65473826286047*^9}, {3.654738473301898*^9, 3.654738519313912*^9}, {
   3.654738551955141*^9, 3.654738559351729*^9}, {3.654738599129403*^9, 
   3.654738599566987*^9}, {3.654738653005966*^9, 3.654738704434928*^9}, {
   3.654738885183668*^9, 3.654738922018218*^9}, {3.654739011880048*^9, 
   3.65473910917871*^9}, {3.654739159170903*^9, 3.654739162120161*^9}, {
   3.6547393230277576`*^9, 3.6547393764227123`*^9}, {3.654739415181912*^9, 
   3.65473942023728*^9}, {3.654739453135948*^9, 3.654739679965816*^9}, {
   3.654739718627033*^9, 3.6547397334487553`*^9}, {3.654739765385715*^9, 
   3.654739851694334*^9}, {3.654739889466123*^9, 3.654740005071364*^9}, {
   3.6547400491757*^9, 3.654740154008595*^9}, 3.654740216700787*^9, {
   3.654740314399178*^9, 3.6547403286014967`*^9}, 3.6547403850818996`*^9, {
   3.654740521314966*^9, 3.654740541208376*^9}, {3.65474063388687*^9, 
   3.654740654515286*^9}, {3.654740701345427*^9, 3.654740810460566*^9}, {
   3.654740848239312*^9, 3.654741082783293*^9}, {3.654741274225429*^9, 
   3.65474127713437*^9}, 3.654741426789631*^9, {3.6547414571147537`*^9, 
   3.654741569824174*^9}, {3.654741604551594*^9, 3.65474161887015*^9}, {
   3.6547416933496304`*^9, 3.654741709459137*^9}, {3.654741935230948*^9, 
   3.654741945307493*^9}, {3.6547830386167917`*^9, 3.654783105454197*^9}, {
   3.6547832406606083`*^9, 3.6547832525310373`*^9}, {3.654783794511154*^9, 
   3.654783837663447*^9}, {3.65478390359721*^9, 3.654783909803577*^9}, {
   3.654783989474423*^9, 3.654784403124803*^9}, {3.654784520313846*^9, 
   3.654784622329711*^9}, {3.6547847231449842`*^9, 3.654784727983326*^9}, {
   3.654785122876478*^9, 3.654785187751635*^9}, {3.654785255283667*^9, 
   3.654785472044736*^9}, {3.654785554330287*^9, 3.6547855634367*^9}, {
   3.6547858133842907`*^9, 3.654785842420493*^9}, {3.654785988696732*^9, 
   3.654785999313698*^9}, {3.654786083383224*^9, 3.654786083827299*^9}, {
   3.654799998454865*^9, 3.654800009014682*^9}, 3.654800058896707*^9, {
   3.6548001027328*^9, 3.654800173381201*^9}, {3.654800206111162*^9, 
   3.654800346038969*^9}, {3.654800415767541*^9, 3.654800685527397*^9}, {
   3.654800717895743*^9, 3.6548007259529123`*^9}, {3.6548007560240192`*^9, 
   3.654800892868227*^9}, {3.654800961067473*^9, 3.6548010443125353`*^9}, {
   3.654801144205307*^9, 3.6548013970409203`*^9}, {3.654801447723054*^9, 
   3.6548014700938597`*^9}, {3.654801562889327*^9, 3.6548017505389137`*^9}, {
   3.6548017965315866`*^9, 3.654801838591158*^9}, {3.654801925163512*^9, 
   3.654801957336872*^9}, {3.654802037838778*^9, 3.654802097667626*^9}, {
   3.6548021338675137`*^9, 3.654802151746142*^9}, {3.6548021938035927`*^9, 
   3.65480226557542*^9}, {3.6548023434469347`*^9, 3.6548024564595747`*^9}, {
   3.654802493167995*^9, 3.654802498462143*^9}, {3.654802632969117*^9, 
   3.654802636326886*^9}, {3.6548026864101667`*^9, 3.6548027624957323`*^9}, {
   3.654802812209731*^9, 3.6548030159498*^9}, {3.6548066359524317`*^9, 
   3.654806720935005*^9}, {3.654806751992766*^9, 3.6548067587144833`*^9}, {
   3.654806821497799*^9, 3.654806889382121*^9}, {3.654807006283039*^9, 
   3.654807210369132*^9}, {3.654807340425975*^9, 3.6548074184588003`*^9}, {
   3.6548074913193197`*^9, 3.654807699931301*^9}, 3.6548078200861998`*^9, {
   3.654807890939335*^9, 3.65480791502777*^9}, {3.654807960842239*^9, 
   3.6548080712336483`*^9}, {3.654808128338978*^9, 3.6548081799548407`*^9}, {
   3.6548101758929577`*^9, 3.654810330104765*^9}, {3.654810401423953*^9, 
   3.654810408457419*^9}, {3.6548104718702717`*^9, 3.6548106690676603`*^9}, {
   3.6548107121438303`*^9, 3.6548108463298893`*^9}, {3.654810878513033*^9, 
   3.6548109250220337`*^9}, {3.654810973064028*^9, 3.6548109963255377`*^9}, {
   3.654811065383171*^9, 3.65481111435124*^9}, {3.6548111568410273`*^9, 
   3.654811165569319*^9}, {3.654811202562601*^9, 3.654811649746052*^9}, {
   3.657067656153129*^9, 3.657067660941557*^9}, {3.6570766750689*^9, 
   3.657076677028944*^9}, {3.658274451899232*^9, 3.6582744912918463`*^9}, {
   3.6583703518545094`*^9, 3.65837035300604*^9}, {3.675682824450754*^9, 
   3.6756828836706333`*^9}, {3.6756829858535852`*^9, 3.675683093476144*^9}, {
   3.675683163431617*^9, 3.675683301926485*^9}, {3.6756834458193197`*^9, 
   3.675683520674423*^9}, {3.675683569012762*^9, 3.675683584065415*^9}, 
   3.675683635500771*^9, {3.675684508273862*^9, 3.675684520687911*^9}, {
   3.675764977565001*^9, 3.675764979617189*^9}, {3.6757662451157312`*^9, 
   3.675766254498831*^9}, {3.675766317871736*^9, 3.675766345394107*^9}, {
   3.6757664078898573`*^9, 3.675766419041957*^9}, {3.6757665260531073`*^9, 
   3.6757665599652433`*^9}, {3.675766688881428*^9, 3.675766693414764*^9}, {
   3.675766760364307*^9, 3.675766898736827*^9}, {3.675766931380547*^9, 
   3.675766932915333*^9}, 3.675767022524777*^9, 3.675767053903921*^9, {
   3.675767094663486*^9, 3.675767157353602*^9}, {3.6757672038039017`*^9, 
   3.67576720815028*^9}, {3.675767263478516*^9, 3.675767302851701*^9}, {
   3.6757673469407463`*^9, 3.6757675158730087`*^9}, {3.675767609117689*^9, 
   3.6757676149377613`*^9}, 3.676110621848937*^9, {3.676110673192341*^9, 
   3.676110708132433*^9}, {3.676110758266004*^9, 3.676110779724792*^9}, {
   3.676110837502541*^9, 3.676110924302658*^9}, {3.676111015370205*^9, 
   3.676111023652515*^9}, {3.676111616884136*^9, 3.676111619101164*^9}, {
   3.67611165132108*^9, 3.67611182175666*^9}, 3.6761124388093357`*^9, {
   3.6761125310577183`*^9, 3.676112535417371*^9}, {3.67611278534865*^9, 
   3.676112852293083*^9}, {3.6761129469420633`*^9, 3.6761129566090517`*^9}, {
   3.6761133261675377`*^9, 3.6761133851728067`*^9}, {3.6761134570078707`*^9, 
   3.6761134886549597`*^9}, {3.676113519585043*^9, 3.676113581404831*^9}, {
   3.676113994051722*^9, 3.6761140495138607`*^9}, {3.67611471759483*^9, 
   3.676114727326268*^9}, {3.676114788083909*^9, 3.676114788267289*^9}, {
   3.67611493328951*^9, 3.6761149644016867`*^9}, {3.6761149993051443`*^9, 
   3.676115038096011*^9}, {3.676115419194528*^9, 3.676115484935451*^9}, {
   3.6761155900865583`*^9, 3.676115605985518*^9}, {3.676115760093829*^9, 
   3.676115778909306*^9}, {3.676115982499289*^9, 3.676116037133542*^9}, {
   3.6761161990700502`*^9, 3.676116233908482*^9}, {3.676116286507206*^9, 
   3.676116286712512*^9}, {3.67611639463448*^9, 3.676116464258135*^9}, {
   3.6761165873897543`*^9, 3.676116602580727*^9}, {3.6761174556753473`*^9, 
   3.67611745672952*^9}, {3.676117506535368*^9, 3.6761175229462357`*^9}, {
   3.676117600215383*^9, 3.6761176442364273`*^9}, {3.676117677849012*^9, 
   3.6761177082717953`*^9}, {3.67611812248459*^9, 3.6761181278825502`*^9}, {
   3.676118580166224*^9, 3.67611860485815*^9}, {3.676118651876894*^9, 
   3.6761187537386017`*^9}, {3.676118838160269*^9, 3.676118838374693*^9}, {
   3.67611887610327*^9, 3.676118940213615*^9}, {3.6761191096213007`*^9, 
   3.676119140757559*^9}, {3.6761192665343122`*^9, 3.676119273783136*^9}, {
   3.67611936142714*^9, 3.676119363325075*^9}, {3.676119488561606*^9, 
   3.676119492487294*^9}, {3.6761200703687267`*^9, 3.6761200707356052`*^9}, {
   3.6761202104386873`*^9, 3.676120268289008*^9}, {3.676120410677075*^9, 
   3.676120443293746*^9}, {3.676120474583932*^9, 3.676120475526285*^9}, {
   3.6761206125852633`*^9, 3.6761206205196333`*^9}, {3.6761234586125507`*^9, 
   3.676123489461384*^9}, {3.6761235483793917`*^9, 3.676123569634534*^9}, {
   3.676124493441423*^9, 3.676124518157651*^9}, {3.676124553443336*^9, 
   3.676124582966502*^9}, {3.6761246820730133`*^9, 3.676124716437358*^9}, {
   3.67612487549886*^9, 3.676124882235661*^9}, {3.676124956893395*^9, 
   3.676124958216114*^9}, {3.6761249904362507`*^9, 3.676124994364197*^9}, {
   3.676125048497746*^9, 3.676125189073619*^9}, {3.676125219923954*^9, 
   3.6761253767254953`*^9}, {3.6761261361020412`*^9, 3.676126180304117*^9}, {
   3.6761262318219957`*^9, 3.676126312208794*^9}, {3.6761263690337877`*^9, 
   3.6761264446350737`*^9}, {3.676126475578731*^9, 3.676126485467072*^9}, {
   3.676126538828438*^9, 3.676126562090301*^9}, {3.676126633785634*^9, 
   3.6761266365895157`*^9}, 3.6761268247458773`*^9, 3.676126912241247*^9, {
   3.676126960168599*^9, 3.676126960170166*^9}, {3.676127961375718*^9, 
   3.676127975299335*^9}, {3.676128268770321*^9, 3.676128269338847*^9}, 
   3.676128811729394*^9, {3.677836754519267*^9, 3.6778367717532873`*^9}, {
   3.6778617429809027`*^9, 3.677861778150968*^9}, {3.677861811363162*^9, 
   3.677861883641371*^9}, {3.677861923205984*^9, 3.677861958483556*^9}, {
   3.677862045631447*^9, 3.6778620815532303`*^9}, {3.677862112947032*^9, 
   3.6778621629685993`*^9}, {3.677922443289874*^9, 3.677922623758004*^9}, 
   3.677923019588035*^9, {3.677923068363806*^9, 3.677923083545373*^9}, {
   3.6779231196089497`*^9, 3.677923586617464*^9}, {3.677923629777021*^9, 
   3.6779236499633503`*^9}, {3.677924129538789*^9, 3.677924181633729*^9}, {
   3.677924307517605*^9, 3.677924330024685*^9}, {3.677924381606667*^9, 
   3.677924497028296*^9}, {3.677924555410111*^9, 3.677924576632324*^9}, 
   3.678027718232277*^9, {3.678096470512413*^9, 3.678096475556388*^9}, 
   3.6784491986905737`*^9, 3.6814510240805492`*^9, {3.682637744857705*^9, 
   3.682637756820936*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"SimulateControl", "[", 
      RowBox[{
      "thetalist_", ",", "dthetalist_", ",", "g_", ",", "Ftipmat_", ",", 
       "Mlist_", ",", "Glist_", ",", "Slist_", ",", "thetamatd_", ",", 
       "dthetamatd_", ",", "ddthetamatd_", ",", "gtilde_", ",", "Mtildelist_",
        ",", "Gtildelist_", ",", "Kp_", ",", "Ki_", ",", "Kd_", ",", "dt_", 
       ",", "intRes_"}], "]"}], ":", "\[IndentingNewLine]", 
     RowBox[{"Takes", " ", "thetalist"}], ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "initial", " ", "joint", " ", 
       "variables"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"dthetalist", ":", 
     RowBox[{"n", "-", 
      RowBox[{
      "vector", " ", "of", " ", "initial", " ", "joint", " ", 
       "velocities"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"g", ":", 
     RowBox[{"Actual", " ", "gravity", " ", "vector", " ", "g"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Ftipmat", ":", 
     RowBox[{
      RowBox[{
      "An", " ", "N", " ", "x", " ", "6", " ", "matrix", " ", "of", " ", 
       "spatial", " ", "forces", " ", "applied", " ", "by", " ", "the", " ", 
       "end"}], "-", 
      RowBox[{"effector", " ", 
       RowBox[{"(", 
        RowBox[{
        "If", " ", "there", " ", "are", " ", "no", " ", "tip", " ", "forces", 
         " ", "the", " ", "user", " ", "should", " ", "input", " ", "a", " ", 
         "zero", " ", "and", " ", "a", " ", "zero", " ", "matrix", " ", 
         "will", " ", "be", " ", "used"}], ")"}]}]}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Mlist", ":", 
     RowBox[{
      RowBox[{
      "Actual", " ", "list", " ", "of", " ", "link", " ", "frames", " ", "i", 
       " ", "relative", " ", "to", " ", "i"}], "-", 
      RowBox[{"1", " ", "at", " ", "the", " ", "home", " ", "position"}]}]}], 
    ",", "\[IndentingNewLine]", 
    RowBox[{"Glist", ":", 
     RowBox[{
     "Actual", " ", "spatial", " ", "inertia", " ", "matrices", " ", "Gi", 
      " ", "of", " ", "the", " ", "links"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Slist", ":", 
     RowBox[{
     "Screw", " ", "axes", " ", "Si", " ", "of", " ", "the", " ", "joints", 
      " ", "in", " ", "a", " ", "space", " ", "frame"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"thetamatd", ":", 
     RowBox[{
     "An", " ", "Nxn", " ", "matrix", " ", "of", " ", "desired", " ", "joint",
       " ", "variables", " ", "from", " ", "the", " ", "reference", " ", 
      "trajectory"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"dthetamatd", ":", 
     RowBox[{
     "An", " ", "Nxn", " ", "matrix", " ", "of", " ", "desired", " ", "joint",
       " ", "velocities"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"ddthetamatd", ":", 
     RowBox[{
     "An", " ", "Nxn", " ", "matrix", " ", "of", " ", "desired", " ", "joint",
       " ", "accelerations"}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"gtilde", ":", 
     RowBox[{"The", " ", 
      RowBox[{"(", 
       RowBox[{"possibly", " ", "incorrect"}], ")"}], " ", "model", " ", "of",
       " ", "the", " ", "gravity", " ", "vector"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Mtildelist", ":", 
     RowBox[{"The", " ", 
      RowBox[{"(", 
       RowBox[{"possibly", " ", "incorrect"}], ")"}], " ", "model", " ", "of",
       " ", "the", " ", "link", " ", "frame", " ", "locations"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Gtildelist", ":", 
     RowBox[{"The", " ", 
      RowBox[{"(", 
       RowBox[{"possibly", " ", "incorrect"}], ")"}], " ", "model", " ", "of",
       " ", "the", " ", "link", " ", "spatial", " ", "inertias"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"Kp", ":", 
     RowBox[{"The", " ", "feedback", " ", "proportional", " ", "gain", " ", 
      RowBox[{"(", 
       RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], 
       ")"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Ki", ":", 
     RowBox[{"The", " ", "feedback", " ", "integral", " ", "gain", " ", 
      RowBox[{"(", 
       RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], 
       ")"}]}]}], ",", "\[IndentingNewLine]", 
    RowBox[{"Kd", ":", 
     RowBox[{"The", " ", "feedback", " ", "derivative", " ", "gain", " ", 
      RowBox[{"(", 
       RowBox[{"identical", " ", "for", " ", "each", " ", "joint"}], 
       ")"}]}]}], ",", 
    RowBox[{"dt", ":", 
     RowBox[{
     "The", " ", "timestep", " ", "between", " ", "points", " ", "on", " ", 
      "the", " ", "reference", " ", "trajectory"}]}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"intRes", ":", 
     RowBox[{
     "Integration", " ", "resolution", " ", "is", " ", "the", " ", "number", 
      " ", "of", " ", "times", " ", "integration", " ", 
      RowBox[{"(", "Euler", ")"}], " ", "takes", " ", "places", "\n", 
      "between", " ", "each", " ", "time", " ", 
      RowBox[{"step", ".", "Must"}], " ", "be", " ", "an", " ", "integer", 
      " ", "value", " ", "greater", " ", "than", " ", "or", " ", "equal", " ",
       "to", " ", "1.", "\[IndentingNewLine]", "\[IndentingNewLine]", 
      "Returns", " ", 
      RowBox[{"taumat", ":", 
       RowBox[{
       "An", " ", "Nxn", " ", "matrix", " ", "of", " ", "the", " ", 
        "controllers", " ", "commanded", " ", "joint", " ", 
        RowBox[{"forces", "/", "torques"}]}]}]}]}], ",", 
    RowBox[{"where", " ", "each", " ", "row", " ", "of", " ", "n", " ", 
     RowBox[{"forces", "/", "torques"}], " ", "corresponds", " ", "to", " ", 
     "a", " ", "single", " ", "time", " ", "instant"}], ",", 
    RowBox[{
     RowBox[{"thetamat", ":", 
      RowBox[{
      "An", " ", "Nxn", " ", "matrix", " ", "of", " ", "actual", " ", "joint",
        " ", 
       RowBox[{"angles", ".", "\[IndentingNewLine]", "The"}], " ", "end", " ",
        "of", " ", "this", " ", "function", " ", "plots", " ", "all", " ", 
       "the", " ", "actual", " ", "and", " ", "desired", " ", "joint", " ", 
       RowBox[{"angles", "."}]}]}], "\[IndentingNewLine]", ";", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{
      "Exapmle", ":", "\[IndentingNewLine]", "Input", ":", 
       "\[IndentingNewLine]", "thetalist"}], "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.1", ",", "0.1"}], "}"}]}], ";", "\n", 
     RowBox[{"dthetalist", "=", 
      RowBox[{"{", 
       RowBox[{"0.1", ",", "0.2", ",", "0.3"}], "}"}]}], ";", "\n", 
     "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Initialise", " ", "robot", " ", "description", " ", 
       RowBox[{"(", 
        RowBox[{"Example", " ", "with", " ", "3", " ", "links"}], ")"}]}], 
      "*)"}], "\n", 
     RowBox[{"g", "=", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "9.8"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
     "\[IndentingNewLine]", 
     RowBox[{"M01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".089159", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".28", ",", ".13585", ",", "0.", ",", "1."}], "}"}]}], 
        "}"}], "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"M23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".1197"}], ",", ".395", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"G1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".010267", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".00666", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.7"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".22689", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".0151074", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.393"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"G3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".0494433", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".004095", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.275"}], 
         "}"}]}], "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"Glist", "=", 
      RowBox[{"{", 
       RowBox[{"G1", ",", "G2", ",", "G3"}], "}"}]}], ";", "\n", 
     RowBox[{"Mlist", "=", 
      RowBox[{"{", 
       RowBox[{"M01", ",", "M12", ",", "M23"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\n", 
     RowBox[{"Slist", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "1.", ",", "0.", ",", "1.", ",", "0.", ",", "1.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", "0."}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.", ",", "1.", ",", "0.", ",", 
          RowBox[{"-", ".089"}], ",", "0.", ",", ".425"}], "}"}]}], "}"}]}], 
     ";", "\[IndentingNewLine]", 
     RowBox[{"dt", " ", "=", "0.01"}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{
      "Create", " ", "a", " ", "trajectory", " ", "to", " ", "follow"}], 
      "*)"}], "\[IndentingNewLine]", 
     RowBox[{"thetaend", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Pi", "/", "2"}], ",", "Pi", ",", 
        RowBox[{"1.5", "*", "Pi"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Tf", "=", "1"}], ";", "\n", 
     RowBox[{"No", "=", 
      RowBox[{"Tf", "/", "dt"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"method", "=", "5"}], ";", "\[IndentingNewLine]", 
     RowBox[{"traj", "=", 
      RowBox[{"JointTrajectory", "[", 
       RowBox[{
       "thetalist", ",", "thetaend", ",", "Tf", ",", "No", ",", "method"}], 
       "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"thetamatd", "=", 
      RowBox[{"{", "thetalist", "}"}]}], ";", "\n", 
     RowBox[{"dthetamatd", "=", 
      RowBox[{"{", "dthetalist", "}"}]}], ";", "\n", 
     RowBox[{"ddthetamatd", "=", 
      RowBox[{"{", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"Do", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"AppendTo", "[", 
         RowBox[{"thetamatd", ",", 
          RowBox[{"traj", "[", 
           RowBox[{"[", "i", "]"}], "]"}]}], "]"}], ";", "\n", 
        RowBox[{"AppendTo", "[", 
         RowBox[{"dthetamatd", ",", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"thetamatd", "[", 
              RowBox[{"[", "i", "]"}], "]"}], "-", 
             RowBox[{"thetamatd", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "1"}], "]"}], "]"}]}], ")"}], "/", "dt"}]}], 
         "]"}], ";", "\n", 
        RowBox[{"AppendTo", "[", 
         RowBox[{"ddthetamatd", ",", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"dthetamatd", "[", 
              RowBox[{"[", "i", "]"}], "]"}], "-", 
             RowBox[{"dthetamatd", "[", 
              RowBox[{"[", 
               RowBox[{"i", "-", "1"}], "]"}], "]"}]}], ")"}], "/", "dt"}]}], 
         "]"}], ";"}], "\[IndentingNewLine]", ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "2", ",", 
         RowBox[{"Length", "[", "traj", "]"}]}], "}"}]}], "]"}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"Possibly", " ", "wrong", " ", "robot", " ", "description", " ", 
       RowBox[{"(", 
        RowBox[{"Example", " ", "with", " ", "3", " ", "links"}], ")"}]}], 
      "*)"}], "\n", 
     RowBox[{"gtilde", "=", 
      RowBox[{"{", 
       RowBox[{"0.8", ",", "0.2", ",", 
        RowBox[{"-", "8.8"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Mhat01", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", ".1", ",", "1."}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"Mhat12", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", 
           RowBox[{"-", "1."}], ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{".3", ",", ".2", ",", "0.", ",", "1."}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\n", 
     RowBox[{"Mhat23", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1.", ",", "0.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "1.", ",", "0.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "1.", ",", "0."}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.", ",", 
           RowBox[{"-", ".2"}], ",", ".4", ",", "1"}], "}"}]}], "}"}], 
       "\[Transpose]"}]}], ";", "\[IndentingNewLine]", "\n", 
     RowBox[{"Gtilde1", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".1", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".1", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".1", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "4.0", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "4.0", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "3.0"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Gtilde2", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".3", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".3", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".1", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "9.0", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "9.5", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "8.0"}], 
         "}"}]}], "}"}]}], ";", "\n", 
     RowBox[{"Gtilde3", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         ".1", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", ".1", ",", "0.", ",", "0.", ",", "0.", ",", "0."}], "}"}],
         ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", ".01", ",", "0.", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "3.0", ",", "0.", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "1.5", ",", "0."}], 
         "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
         "0.", ",", "0.", ",", "0.", ",", "0.", ",", "0.", ",", "2.0"}], 
         "}"}]}], "}"}]}], ";", "\n", "\[IndentingNewLine]", 
     RowBox[{"Gtildelist", "=", 
      RowBox[{"{", 
       RowBox[{"Gtilde1", ",", "Gtilde2", ",", "Gtilde3"}], "}"}]}], ";", 
     "\n", 
     RowBox[{"Mtildelist", "=", 
      RowBox[{"{", 
       RowBox[{"Mhat01", ",", "Mhat12", ",", "Mhat23"}], "}"}]}], ";", 
     "\[IndentingNewLine]", "\[IndentingNewLine]", "\n", 
     RowBox[{"Ftipmat", "=", 
      RowBox[{"ConstantArray", "[", 
       RowBox[{"1", ",", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Length", "[", "traj", "]"}], ",", "6"}], "}"}]}], "]"}]}], 
     ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
     RowBox[{"Kp", "=", "20"}], ";", "\n", 
     RowBox[{"Ki", "=", "10"}], ";", "\n", 
     RowBox[{"Kd", "=", "18"}], ";", "\[IndentingNewLine]", 
     RowBox[{"intRes", "=", "8"}], ";", "\[IndentingNewLine]", 
     "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"taumat", ",", "thetamat"}], "}"}], " ", "=", " ", 
      RowBox[{"SimulateControl", "[", 
       RowBox[{
       "thetalist", ",", "dthetalist", ",", "g", ",", "Ftipmat", ",", "Mlist",
         ",", "Glist", ",", "Slist", ",", "thetamatd", ",", "dthetamatd", ",",
         "ddthetamatd", ",", "gtilde", ",", "Mtildelist", ",", "Gtildelist", 
        ",", "Kp", ",", "Ki", ",", "Kd", ",", "dt", ",", "intRes"}], "]"}]}], 
     ";"}]}], "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"SimulateControl", "[", 
     RowBox[{
     "thetalist_", ",", "dthetalist_", ",", "g_", ",", "Ftipmat_", ",", 
      "Mlist_", ",", "Glist_", ",", "Slist_", ",", "thetamatd_", ",", 
      "dthetamatd_", ",", "ddthetamatd_", ",", "gtilde_", ",", "Mtildelist_", 
      ",", "Gtildelist_", ",", "Kp_", ",", "Ki_", ",", "Kd_", ",", "dt_", ",",
       "intRes_"}], "]"}], ":=", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "taumat", ",", "thetamat", ",", "n", ",", "thetacurrent", ",", " ", 
        "dthetacurrent", ",", " ", "eint", ",", " ", "taulist", ",", 
        "ddthetalist", ",", " ", "NewFtipmat", ",", " ", "links", ",", " ", 
        "plotAngles", ",", " ", "labels", ",", "style", ",", "col"}], "}"}], 
      ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"n", "=", 
        RowBox[{"Length", "[", "thetamatd", "]"}]}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"NewFtipmat", " ", "=", " ", "Ftipmat"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"If", "[", 
        RowBox[{
         RowBox[{"Ftipmat", " ", "\[Equal]", " ", "0"}], ",", " ", 
         RowBox[{"NewFtipmat", " ", "=", " ", 
          RowBox[{"ConstantArray", "[", 
           RowBox[{"0", ",", 
            RowBox[{"{", 
             RowBox[{"n", ",", "6"}], "}"}]}], "]"}]}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"thetacurrent", "=", "thetalist"}], ";", "\n", 
       RowBox[{"dthetacurrent", "=", "dthetalist"}], ";", "\n", 
       RowBox[{"eint", "=", "0"}], ";", "\n", 
       RowBox[{"taumat", "=", 
        RowBox[{"{", "}"}]}], ";", "\n", 
       RowBox[{"thetamat", "=", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"taulist", "=", 
        RowBox[{"ComputedTorque", "[", 
         RowBox[{
         "thetacurrent", ",", "dthetacurrent", ",", "eint", ",", "gtilde", 
          ",", "Mtildelist", ",", "Gtildelist", ",", "Slist", ",", 
          RowBox[{"thetamatd", "[", 
           RowBox[{"[", "1", "]"}], "]"}], ",", 
          RowBox[{"dthetamatd", "[", 
           RowBox[{"[", "1", "]"}], "]"}], ",", 
          RowBox[{"ddthetamatd", "[", 
           RowBox[{"[", "1", "]"}], "]"}], ",", "Kp", ",", "Ki", ",", "Kd"}], 
         "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{"ddthetalist", "=", 
            RowBox[{"ForwardDynamics", "[", 
             RowBox[{
             "thetacurrent", ",", "dthetacurrent", ",", "taulist", ",", "g", 
              ",", 
              RowBox[{"NewFtipmat", "[", 
               RowBox[{"[", "1", "]"}], "]"}], ",", "Mlist", ",", "Glist", 
              ",", "Slist"}], "]"}]}], ";", "\n", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{"thetacurrent", ",", "dthetacurrent"}], "}"}], "=", 
            RowBox[{"EulerStep", "[", 
             RowBox[{
             "thetacurrent", ",", "dthetacurrent", ",", "ddthetalist", ",", 
              RowBox[{"(", 
               RowBox[{"dt", "/", "intRes"}], ")"}]}], "]"}]}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"taumat", ",", "taulist"}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"thetamat", ",", "thetacurrent"}], "]"}], ";"}], 
          "\[IndentingNewLine]", "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"j", ",", "1", ",", "intRes"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"eint", "=", 
        RowBox[{"eint", "+", 
         RowBox[{"(", 
          RowBox[{"dt", "*", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"thetamatd", "[", 
              RowBox[{"[", "1", "]"}], "]"}], "-", "thetacurrent"}], ")"}]}], 
          ")"}]}]}], ";", "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{"taulist", "=", 
            RowBox[{"ComputedTorque", "[", 
             RowBox[{
             "thetacurrent", ",", "dthetacurrent", ",", "eint", ",", "gtilde",
               ",", "Mtildelist", ",", "Gtildelist", ",", "Slist", ",", 
              RowBox[{"thetamatd", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", 
              RowBox[{"dthetamatd", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", 
              RowBox[{"ddthetamatd", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ",", "Kp", ",", "Ki", ",", 
              "Kd"}], "]"}]}], ";", "\[IndentingNewLine]", 
           RowBox[{"Do", "[", 
            RowBox[{
             RowBox[{"{", "\[IndentingNewLine]", 
              RowBox[{
               RowBox[{"ddthetalist", "=", 
                RowBox[{"ForwardDynamics", "[", 
                 RowBox[{
                 "thetacurrent", ",", "dthetacurrent", ",", "taulist", ",", 
                  "g", ",", 
                  RowBox[{"NewFtipmat", "[", 
                   RowBox[{"[", "i", "]"}], "]"}], ",", "Mlist", ",", "Glist",
                   ",", "Slist"}], "]"}]}], ";", "\n", 
               RowBox[{
                RowBox[{"{", 
                 RowBox[{"thetacurrent", ",", "dthetacurrent"}], "}"}], "=", 
                RowBox[{"EulerStep", "[", 
                 RowBox[{
                 "thetacurrent", ",", "dthetacurrent", ",", "ddthetalist", 
                  ",", 
                  RowBox[{"(", 
                   RowBox[{"dt", "/", "intRes"}], ")"}]}], "]"}]}], ";", 
               "\[IndentingNewLine]", 
               RowBox[{"AppendTo", "[", 
                RowBox[{"taumat", ",", "taulist"}], "]"}], ";", 
               "\[IndentingNewLine]", 
               RowBox[{"AppendTo", "[", 
                RowBox[{"thetamat", ",", "thetacurrent"}], "]"}], ";"}], 
              "\[IndentingNewLine]", "}"}], ",", 
             RowBox[{"{", 
              RowBox[{"j", ",", "1", ",", "intRes"}], "}"}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"eint", "=", 
            RowBox[{"eint", "+", 
             RowBox[{"(", 
              RowBox[{"dt", "*", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"thetamatd", "[", 
                  RowBox[{"[", "i", "]"}], "]"}], "-", "thetacurrent"}], 
                ")"}]}], ")"}]}]}], ";"}], "\[IndentingNewLine]", "}"}], ",", 
         
         RowBox[{"{", 
          RowBox[{"i", ",", "2", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{"Output", " ", "plotting"}], "*)"}], "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
        "Resize", " ", "thetamatd", " ", "to", " ", "be", " ", "intRes", " ", 
         "times", " ", "bigger", " ", "so", " ", "it", " ", "plots", " ", 
         "well", " ", "with", " ", "thetamat"}], "*)"}], 
       "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
        "Run", " ", "through", " ", "it", " ", "all", " ", "and", " ", 
         "insert", " ", "intRes", " ", "times", " ", "the", " ", "amount", 
         " ", "in", " ", "each", " ", "position"}], "*)"}], 
       "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{"Do", "[", 
          RowBox[{
           RowBox[{"{", "\[IndentingNewLine]", 
            RowBox[{
             RowBox[{"Do", "[", 
              RowBox[{
               RowBox[{"{", "\[IndentingNewLine]", 
                RowBox[{
                 RowBox[{"thetamatd", " ", "=", " ", 
                  RowBox[{"Insert", "[", 
                   RowBox[{"thetamatd", ",", 
                    RowBox[{"thetamatd", "[", 
                    RowBox[{"[", "i", "]"}], "]"}], ",", 
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{"i", "*", "intRes"}], ")"}], "+", "j", "-", 
                    "1"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", "}"}], 
               ",", 
               RowBox[{"{", 
                RowBox[{"j", ",", "1", ",", "intRes"}], "}"}]}], "]"}], ";"}],
             "\[IndentingNewLine]", "}"}], ",", 
           RowBox[{"{", 
            RowBox[{"i", ",", "1", ",", "n"}], "}"}]}], "]"}], ";"}], 
        "\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]", 
       RowBox[{"(*", 
        RowBox[{
         RowBox[{
         "Resize", " ", "by", " ", "looping", " ", "and", " ", "just", " ", 
          "appending", " ", "to", " ", "a", " ", "new", " ", "matrix"}], ",", 
         " ", 
         RowBox[{
         "easier", " ", "than", " ", "just", " ", "edditing", " ", "the", " ",
           "old", " ", "one"}]}], "*)"}], "\[IndentingNewLine]", 
       RowBox[{"thetamatdResized", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{"Do", "[", 
            RowBox[{
             RowBox[{"{", "\[IndentingNewLine]", 
              RowBox[{
               RowBox[{"AppendTo", "[", 
                RowBox[{"thetamatdResized", ",", 
                 RowBox[{"thetamatd", "[", 
                  RowBox[{"[", "i", "]"}], "]"}]}], "]"}], ";"}], 
              "\[IndentingNewLine]", "}"}], ",", 
             RowBox[{"{", 
              RowBox[{"j", ",", "1", ",", "intRes"}], "}"}]}], "]"}], ";"}], 
          "\[IndentingNewLine]", "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "n"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", "\[IndentingNewLine]", 
       RowBox[{"links", "=", 
        RowBox[{"Length", "[", 
         RowBox[{"thetamat", "[", 
          RowBox[{"[", "1", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"plotAngles", " ", "=", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"labels", " ", "=", " ", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"style", "=", 
        RowBox[{"{", "}"}]}], ";", "\[IndentingNewLine]", 
       RowBox[{"Do", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"AppendTo", "[", 
            RowBox[{"plotAngles", ",", " ", 
             RowBox[{"thetamat", "[", 
              RowBox[{"[", 
               RowBox[{"All", ",", "i"}], "]"}], "]"}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"plotAngles", ",", " ", 
             RowBox[{"thetamatdResized", "[", 
              RowBox[{"[", 
               RowBox[{"All", ",", "i"}], "]"}], "]"}]}], "]"}], ";", 
           "\[IndentingNewLine]", "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"labels", ",", " ", 
             RowBox[{"StringJoin", "[", 
              RowBox[{"\"\<ActualTheta\>\"", ",", 
               RowBox[{"ToString", "[", "i", "]"}]}], "]"}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"labels", ",", " ", 
             RowBox[{"StringJoin", "[", 
              RowBox[{"\"\<DesiredTheta\>\"", ",", 
               RowBox[{"ToString", "[", "i", "]"}]}], "]"}]}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"col", " ", "=", " ", 
            RowBox[{"RandomColor", "[", "]"}]}], ";", "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"style", ",", " ", "col"}], "]"}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"AppendTo", "[", 
            RowBox[{"style", ",", " ", 
             RowBox[{"{", 
              RowBox[{"col", ",", "Dashed"}], "}"}]}], "]"}], ";"}], 
          "\[IndentingNewLine]", "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "links"}], "}"}]}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Print", "[", 
        RowBox[{"ListLinePlot", "[", 
         RowBox[{"plotAngles", ",", 
          RowBox[{"PlotLegends", "\[Rule]", "labels"}], ",", 
          RowBox[{"PlotStyle", "\[Rule]", "style"}], ",", 
          RowBox[{"AxesLabel", "\[Rule]", 
           RowBox[{"{", 
            RowBox[{"\"\<Time\>\"", ",", "\"\<Joint Angles\>\""}], "}"}]}], 
          ",", 
          RowBox[{
          "PlotLabel", "\[Rule]", 
           "\"\<Plot of Actual and Desired Joint Angles\>\""}], ",", 
          RowBox[{"PlotRange", "\[Rule]", "Full"}]}], "]"}], "]"}], ";", 
       "\[IndentingNewLine]", 
       RowBox[{"Return", "[", 
        RowBox[{"{", 
         RowBox[{"taumat", ",", "thetamat"}], "}"}], "]"}], ";"}]}], 
     "\[IndentingNewLine]", "]"}]}], "\[IndentingNewLine]", 
   "\[IndentingNewLine]", "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{
  3.677924853965807*^9, 3.6779251590971527`*^9, {3.677925359632638*^9, 
   3.677925494612109*^9}, {3.677925600073634*^9, 3.677925626646657*^9}, {
   3.6779257311273537`*^9, 3.677925754253189*^9}, {3.677925821586344*^9, 
   3.6779258481817217`*^9}, {3.6779273195920897`*^9, 3.677927355304405*^9}, {
   3.6779277973487988`*^9, 3.6779278021979847`*^9}, {3.67792783288631*^9, 
   3.677928121348311*^9}, {3.6779281661484547`*^9, 3.6779281914512463`*^9}, {
   3.677928236881343*^9, 3.677928274073283*^9}, {3.677932699269586*^9, 
   3.677932700327119*^9}, {3.677934027976076*^9, 3.67793404625205*^9}, {
   3.677934083734558*^9, 3.677934088566362*^9}, {3.677934163992302*^9, 
   3.677934185281588*^9}, {3.677934364550599*^9, 3.677934419897183*^9}, {
   3.6779344508362417`*^9, 3.677934451566586*^9}, {3.677934492157349*^9, 
   3.677934494681019*^9}, {3.677934527031344*^9, 3.677934527237537*^9}, {
   3.677934575273864*^9, 3.677934644837254*^9}, {3.677934691786611*^9, 
   3.677934817811751*^9}, {3.6779349436710033`*^9, 3.677934985204653*^9}, {
   3.677937983500269*^9, 3.677937984977446*^9}, {3.6779381257429047`*^9, 
   3.677938133660923*^9}, {3.677938202196999*^9, 3.677938227703239*^9}, 
   3.677938377314639*^9, {3.677938438157711*^9, 3.677938576047544*^9}, {
   3.678096486746469*^9, 3.678096488807798*^9}, {3.678458049947278*^9, 
   3.678458060952914*^9}, 3.6784581253393917`*^9, 3.678458183955419*^9, {
   3.6784582141901493`*^9, 3.678458265956141*^9}, {3.6784583117105293`*^9, 
   3.678458357793298*^9}, {3.6814552043709087`*^9, 3.6814552145069237`*^9}, {
   3.681455255663412*^9, 3.6814552687776537`*^9}, {3.682527789069289*^9, 
   3.68252787877598*^9}, {3.6826377575249643`*^9, 3.682637758148562*^9}, {
   3.682637818965764*^9, 3.682637876057115*^9}, {3.68263799461088*^9, 
   3.6826380144271793`*^9}, {3.682638200804899*^9, 3.682638250258004*^9}, {
   3.682638491207387*^9, 3.682638509228066*^9}, {3.682638631917736*^9, 
   3.682638693519517*^9}, {3.682638745721383*^9, 3.682638747825354*^9}, {
   3.682638856341588*^9, 3.6826388905407257`*^9}, {3.682640381499113*^9, 
   3.6826403886003857`*^9}, {3.683217656988469*^9, 3.6832176852212048`*^9}, {
   3.683217744924802*^9, 3.683217766049675*^9}, {3.683217920478265*^9, 
   3.683217969778751*^9}, {3.683218016468755*^9, 3.683218027518409*^9}, {
   3.683218299683257*^9, 3.683218343811051*^9}, {3.6832294792369823`*^9, 
   3.6832295082177353`*^9}, {3.683229548795042*^9, 3.683229601725254*^9}, {
   3.6832296530608*^9, 3.683229663552986*^9}, {3.683230807268846*^9, 
   3.683230812968185*^9}, {3.683231175975581*^9, 3.683231188638826*^9}, {
   3.6832842444241858`*^9, 3.6832842861726007`*^9}, {3.68328437230928*^9, 
   3.683284428369204*^9}, {3.683284725151619*^9, 3.683284747446712*^9}, {
   3.683284795412685*^9, 3.683284800591009*^9}, {3.683284836456174*^9, 
   3.6832849599694633`*^9}, {3.6832850755968657`*^9, 3.683285079362492*^9}, {
   3.683285131777463*^9, 3.6832851358839684`*^9}, {3.683285170074172*^9, 
   3.683285181469409*^9}, {3.6832852291753883`*^9, 3.683285255535644*^9}, {
   3.68328532480322*^9, 3.683285387297305*^9}, {3.683293799303421*^9, 
   3.683293808848403*^9}, {3.683293900157916*^9, 3.6832939365266323`*^9}, {
   3.683294004515588*^9, 3.683294015304785*^9}, {3.6832940532275763`*^9, 
   3.6832940548821487`*^9}, {3.683294119846901*^9, 3.683294124792877*^9}, {
   3.683294201505858*^9, 3.683294206793943*^9}, {3.683294527397341*^9, 
   3.683294536674127*^9}, {3.68329459453303*^9, 3.6832946335870047`*^9}, {
   3.683294675202341*^9, 3.68329467563385*^9}, {3.683294828461928*^9, 
   3.683294843524208*^9}, {3.683295063325433*^9, 3.683295096414126*^9}, {
   3.6832951835909977`*^9, 3.68329518390718*^9}, {3.683295296841012*^9, 
   3.6832953163258457`*^9}, {3.683295372115971*^9, 3.683295388765071*^9}, {
   3.683295527674631*^9, 3.683295532339222*^9}, {3.683295569113285*^9, 
   3.683295606563596*^9}, 3.683295882014827*^9, {3.683296409908462*^9, 
   3.68329642375393*^9}, {3.683296552745085*^9, 3.683296592250246*^9}, {
   3.683296653009379*^9, 3.683296669019459*^9}, {3.683296989986912*^9, 
   3.683297034939659*^9}, {3.683298384693363*^9, 3.68329838977964*^9}, {
   3.683298428043923*^9, 3.6832984401731453`*^9}, {3.683298491152433*^9, 
   3.683298544421233*^9}, {3.683298597144882*^9, 3.683298646838909*^9}, {
   3.683298685695128*^9, 3.683298691211185*^9}, 3.6832988974154367`*^9, 
   3.6833032529549294`*^9, 3.683303395645307*^9, {3.6833040545975246`*^9, 
   3.683304084731357*^9}, {3.683307930454728*^9, 3.6833079832892313`*^9}, {
   3.683308115214395*^9, 3.683308196130604*^9}}]
}, Closed]]
},
WindowSize->{1855, 1028},
WindowMargins->{{-10, Automatic}, {Automatic, -10}},
FrontEndVersion->"10.2 for Linux x86 (64-bit) (July 29, 2015)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[580, 22, 657, 16, 138, "Title"],
Cell[1240, 40, 1783, 35, 400, "Title"],
Cell[3026, 77, 331, 5, 55, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[3394, 87, 348, 6, 72, "Title"],
Cell[3745, 95, 2210, 58, 209, "Input"],
Cell[5958, 155, 2904, 78, 231, "Input"],
Cell[8865, 235, 2414, 64, 231, "Input"],
Cell[11282, 301, 2611, 58, 253, "Input"],
Cell[13896, 361, 4462, 107, 288, "Input"],
Cell[18361, 470, 8003, 199, 341, "Input"],
Cell[26367, 671, 11248, 188, 310, "Input"],
Cell[37618, 861, 11358, 188, 297, "Input"],
Cell[48979, 1051, 12077, 211, 354, "Input"],
Cell[61059, 1264, 11434, 195, 332, "Input"],
Cell[72496, 1461, 11264, 187, 275, "Input"],
Cell[83763, 1650, 11981, 209, 374, "Input"],
Cell[95747, 1861, 12299, 213, 324, "Input"],
Cell[108049, 2076, 12187, 213, 297, "Input"],
Cell[120239, 2291, 16746, 320, 530, "Input"],
Cell[136988, 2613, 31174, 680, 693, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[168199, 3298, 301, 6, 46, "Title"],
Cell[168503, 3306, 6200, 151, 486, "Input"],
Cell[174706, 3459, 5976, 149, 486, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[180719, 3613, 364, 6, 46, "Title"],
Cell[181086, 3621, 15222, 280, 704, "Input"],
Cell[196311, 3903, 14688, 263, 638, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[211036, 4171, 402, 7, 46, "Title"],
Cell[211441, 4180, 25059, 466, 1155, "Input"],
Cell[236503, 4648, 25409, 463, 1155, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[261949, 5116, 453, 8, 46, "Title"],
Cell[262405, 5126, 11361, 209, 528, "Input"],
Cell[273769, 5337, 34423, 762, 1639, "Input"],
Cell[308195, 6101, 17984, 375, 1036, "Input"],
Cell[326182, 6478, 16032, 333, 913, "Input"],
Cell[342217, 6813, 15960, 335, 913, "Input"],
Cell[358180, 7150, 9813, 256, 891, "Input"],
Cell[367996, 7408, 17510, 373, 979, "Input"],
Cell[385509, 7783, 10011, 176, 539, "Input"],
Cell[395523, 7961, 22738, 493, 1551, "Input"],
Cell[418264, 8456, 25926, 552, 1661, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[444227, 9013, 456, 8, 46, "Title"],
Cell[444686, 9023, 15853, 253, 517, "Input"],
Cell[460542, 9278, 3588, 93, 539, "Input"],
Cell[464133, 9373, 16639, 300, 924, "Input"],
Cell[480775, 9675, 19595, 377, 1185, "Input"],
Cell[500373, 10054, 22054, 429, 1229, "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell[522464, 10488, 447, 8, 46, "Title"],
Cell[522914, 10498, 26076, 498, 1441, "Input"],
Cell[548993, 10998, 37774, 872, 3333, "Input"]
}, Closed]]
}
]
*)

(* End of internal cache information *)
